Moderator: dromia
Code: Select all
/************************************************
Arduino Example Code to send PPM servo signals via 4017 decade counter chip.
-- HWServo4017.pde --
Created: 9/30/10
Last Modified: 11/08/2010
Written by Petey the Programmer
************************************************/
#include <ctype.h>
#include <math.h>
#include <avr/io.h>
#define ON 1
#define OFF 0
#define OneSecond 1000
#define ledPin 13
// --------------------------------------------------------------------------------------------------------------------------------------------
void Servo_4017_init( void );
void Servo_4017_Set_Pulse_Width( int ServoNum, int pulseWidth );
// --------------------------------------------------------------------------------------------------------------------------------------------
/* Global Variable definitions */
uint16_t sWidth[8], sInc[8];
unsigned long myTimer = 0; // general purpose timer
unsigned long OneSecTimer = 0;
int LED_On = 0;
// --------------------------------------------------------------------------------------------------------------------------------------------
// Data for dancing sequence
int PWM_SEQ[4][8] = {
{
2000,2000,2000,2000,2000,2000,2000,2000 }
,
{
1000,1000,1000,1000,1000,1000,1000,1000 }
,
{
1000,1000,1000,1000,2000,2000,2000,2000 }
,
{
2000,2000,2000,2000,1000,1000,1000,1000 }
};
// --------------------------------------------------------------------------------------------------------------------------------------------
void blink_led(void)
{
int n;
for (n = 0; n < 5; n++) {
digitalWrite(ledPin, 0);
delay(20);
digitalWrite(ledPin,1);
delay(20);
}
}
// -----------------------------------------------------------------------------------------------------
void setup()
{
int n;
Serial.begin(19200); // debug output over serial line
pinMode(ledPin, OUTPUT);
digitalWrite(ledPin, ON);
LED_On = 1;
delay(OneSecond);
Serial.println("---");
Serial.println("Arduino 4017 Servo Driver v1.0");
delay(OneSecond);
digitalWrite(ledPin, OFF);
for (n=0; n<8; n++) // Set all servos to their center positions.
{
sWidth[n] = 1500;
sInc[n] = n * 3; // setup each servo to move at a different speed
}
sInc[0] = 2;
Servo_4017_init(); /* Init Timer1 and setup Output Compare interrupt */
blink_led();
digitalWrite(ledPin, ON);
OneSecTimer = millis();
myTimer = millis();
}
// -----------------------------------------------------------------------------------------------------
// Every 100 ms, change the pulse width being sent to each servo.
// Interrupt routines grab the sWidth array elements as it needs them.
void precision_loop ()
{
int n;
if ((millis() - myTimer) >= 20) // 100ms is arbitrary interval for demo
{
myTimer = millis();
for (n=0; n<8; n++) // Keep servo 0 at center position - move all the rest. Demos NO jitter on Servo 0.
{
sWidth[n] += sInc[n];
Servo_4017_Set_Pulse_Width(n, sWidth[n]);
if ((sWidth[n] > 2100) || (sWidth[n] < 900)) // reverse direction at end of travel
sInc[n] = -sInc[n];
}
}
if ((millis() - OneSecTimer) >= OneSecond) // this goes off once per second
{
OneSecTimer = millis();
LED_On = !LED_On;
digitalWrite(ledPin, LED_On);
}
}
// -----------------------------------------------------------------------------------------------------
// from ArduPilot Mega Dance demo
void dance_loop()
{
for(int w=0; w<=5; w++)
{
for(int y=0; y<2; y++)
{
for(int x=0; x<=7; x++)
{
Servo_4017_Set_Pulse_Width(x, PWM_SEQ[y][x]);
delay(100);
}
}
}
/* -------- */
for(int w=0; w<=5; w++)
{
for(int y=2; y<4; y++)
{
for(int x=0; x<=7; x++)
{
Servo_4017_Set_Pulse_Width(x, PWM_SEQ[y][x]);
delay(1);
}
delay(300);
}
}
}
// -----------------------------------------------------------------------------------------------------
// only let one loop run at a time.
// comment out the other.
void loop()
{
if (1==1)
dance_loop();
else
precision_loop();
}
// -----------------------------------------------------------------------------------------------------
// End of File
Code: Select all
/* $Id: servos_4017.pde 100 2010-11-08 00:00:00Z petey $
*
* (c) 2003-2006 Pascal Brisset, Antoine Drouin
*
* This file was part of paparazzi.
* Updated for Arduino by Petey the Programmer
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#include <avr/io.h>
#include <avr/interrupt.h>
// ----------------------------------------------------------------
/* Connections to 4017 chip */
// On the 4017 Chip, "Enable" pin 13 is held low in hardware (hard-wired to ground)
// Arduino Pin 9 = ATMega328 PortB-1 -> connects to 4017 Clock Input pin 14 - PWM pin
// Arduino Pin 8 = ATMega328 PortB-0 -> connects to 4017 Reset pin 15 (also called Clear)
#define HW_4017_RESET_PIN 8
#define HW_4017_CLOCK_PIN 9
#define _4017_RESET_PORT PORTB
#define _4017_CLOCK_PORT PORTB
#define _4017_RESET_DDR DDRB
#define _4017_CLOCK_DDR DDRB
#define _4017_RESET_PIN PINB0
#define _4017_CLOCK_PIN PINB1
// ----------------------------------------------------------------------------------------------------
// Output Compare registers and setup bits
#define SERVO_OCR OCR1A
#define SERVO_ENABLE OCIE1A
#define SERVO_FLAG OCF1A
#define SERVO_FORCE FOC1A
#define SERVO_COM0 COM1A0
#define SERVO_COM1 COM1A1
// ----------------------------------------------------------------------------------------------------
// Timer1 registers
#if defined (__AVR_ATmega8__)
#define TIMER_SK TIMSK
#define TIMER_IFR TIFR
#define FORCE_REG TCCR1A
#else
#define TIMER_SK TIMSK1
#define TIMER_IFR TIFR1
#define FORCE_REG TCCR1C
#endif
// ----------------------------------------------------------------
#ifndef _BV
#define _BV(bit) (1 << (bit))
#endif
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
// ----------------------------------------------------------------
#define CLOCK 16
#define SYS_TICS_OF_USEC(us) (uint16_t)((us)*CLOCK)
#define Chop(_x, _min, _max) ( (_x) < (_min) ? (_min) : (_x) > (_max) ? (_max) : (_x) )
#define SetServo(x, v) servo_widths[x] = SYS_TICS_OF_USEC(Chop(v,700,2400));
// ----------------------------------------------------------------
/* servo_widths array holds the servo pulse widths in clock ticks */
#define _4017_NB_CHANNELS 10
unsigned int servo_widths[_4017_NB_CHANNELS];
void Servo_4017_Set_Pulse_Width( int ServoNum, int pulseWidth )
{
servo_widths[ServoNum] = SYS_TICS_OF_USEC(pulseWidth);
}
// --------------------------------------------------------------------------------------------------------
/*
* We use the output compare registers to generate our servo pulses.
* These should be connected to a decade counter that routes the
* pulses to the appropriate servo.
*
* Initialization involves:
*
* - Reseting the decade counters
* - Writing the first pulse width to the counters
* - Setting output compare to set the clock line by calling servo_enable()
* - Bringing down the reset lines
*
* Ideally, you can use two decade counters to drive 20 servos.
*/
void Servo_4017_init( void ) {
uint8_t i;
/* Configure 2 Arduino pins as outputs for controlling the 4017 reset and clock lines */
digitalWrite(HW_4017_RESET_PIN, 0);
digitalWrite(HW_4017_CLOCK_PIN, 0);
pinMode(HW_4017_RESET_PIN, OUTPUT);
pinMode(HW_4017_CLOCK_PIN, OUTPUT);
digitalWrite(HW_4017_RESET_PIN, 1); // Reset the decade counter
digitalWrite(HW_4017_CLOCK_PIN, 0); // Lower the clock line
/* Set all servos at their midpoints */
for( i=0 ; i < _4017_NB_CHANNELS ; i++ )
servo_widths[i] = SYS_TICS_OF_USEC(1500); // SetServo(i,1500);
// -----------------------------------------------------------------------------------------
// Configure Timer 1 and setup the Output Compare registers...
/* Timer1 @ Clk/1: System clock */
TCCR1A = 0x00;
TCCR1B = _BV(CS10);
SERVO_OCR = 0x7FFF; /* Set servos to go off some long time from now */
TCCR1A |= _BV(SERVO_COM0 ); /* Set output compare to toggle the output bits */
#ifdef SERVOS_FALLING_EDGE
/* Starts CLOCK high for the falling edge case */
FORCE_REG |= _BV(SERVO_FORCE);
// Serial.println("Falling Edge Active!");
#endif
/* Clear the interrupt flags in case they are set */
TIMER_IFR = _BV(SERVO_FLAG);
digitalWrite(HW_4017_RESET_PIN, 0); // Lower the decade counter reset line to start it running
/* Enable our output compare interrupts */
TIMER_SK |= _BV(SERVO_ENABLE);
}
/* -----------------------------------------------------------------------------------------------------
* Interrupt routine
*
* write the next pulse width to OCR register and
* assert the servo signal. It will be cleared by
* the following compare match.
* ----------------------------------------------------------------------------------------------------- */
ISR( TIMER1_COMPA_vect )
{
static uint8_t servo = 0;
uint16_t width;
#ifdef SERVOS_FALLING_EDGE
#define RESET_WIDTH SYS_TICS_OF_USEC(1000)
#define FIRST_PULSE_WIDTH SYS_TICS_OF_USEC(100)
/** The clock pin has been initialized high and is toggled down by the timer.
Unfortunately it seems that reset does not work on 4017 in this case if it
occurs after the first falling edge. We add two more states at the end of
the sequence:
- keeping clock low, reset high during 1ms
- clock high (toggled by the timer), reset down, during 100us (looks like
the first pulse of a standard RC */
if (servo == _4017_NB_CHANNELS) {
sbi( _4017_RESET_PORT, _4017_RESET_PIN ); // set Reset pin high
/** Start a long 1ms reset, keep clock low */
SERVO_OCR += RESET_WIDTH;
servo++;
return;
}
if (servo > _4017_NB_CHANNELS) {
/** Clear the reset, the clock has been toggled high */
cbi( _4017_RESET_PORT, _4017_RESET_PIN ); // clear reset pin
/** Start a short pulse-like period */
SERVO_OCR += FIRST_PULSE_WIDTH;
servo = 0; /* Starts a new sequence of pulses next time */
return;
}
#else
if (servo >= _4017_NB_CHANNELS) {
sbi( _4017_RESET_PORT, _4017_RESET_PIN ); // set reset pin High
servo = 0;
// FIXME: 500 ns required by 4017 reset ???? why does it work without!
//asm( "nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop;" );
cbi( _4017_RESET_PORT, _4017_RESET_PIN ); // clear reset pin
}
#endif
width = servo_widths[servo];
SERVO_OCR += width;
FORCE_REG |= _BV(SERVO_FORCE);
servo++;
}
// --------------------------------------------------------------------------------------------------------------
// End of FileDo you have a separate power supply for the servos? I wonder if the extra power available over USB is making the difference. Does it work if you disconnect a few of the servos?Demonic69 wrote:I've not adapted the code yet as I can't get it working without being connected to the PC via USB, which is odd. I can't figure out where in the code is stopping me getting this working standalone either, maybe one of you smarty-pants can see what I'm missing.
You don't need to be a member to shoot in the main meetings, there's just a 'non-member additional fee' when you enter...Demonic69 wrote: I'm not a member so they might not let ruffians like me in :-)
Users browsing this forum: No registered users and 1 guest