motion.c

/****************************************************************

Code by Rue_mohr, Mar 16 2008

wrapper for core functionality of a servo position controller
the lines are blurred

9600 N81
servos go on port A
software handles 8 of them

control as per http://www.lavrsen.dk/twiki/bin/view/Motion/MotionTrackerAPI

version 6 adds commands to read back the positions

*****************************************************************/


//-----| Include Files |-------

// this is a link to the prycon lib for sending serial data
#define SendByte(A)    uartSendByte(A)

#define SetBit(BIT, PORT)     PORT |= (1<<BIT)
#define ClearBit(BIT, PORT)   PORT &= ~(1<<BIT)
#define IsSet(BIT, PORT)    (PORT & (1<<BIT)) != 0
#define IsLow(BIT, PORT)     (PORT & (1<<BIT)) == 0



#define Button0Bit   0
#define Button0Port  PINC
#define IsDown0()    IsLow(Button0Bit,Button0Port)

#define Button1Bit   1
#define Button1Port  PINC
#define IsDown1()    IsLow(Button1Bit,Button0Port)

#define Button2Bit   2
#define Button2Port  PINC
#define IsDown2()    IsLow(Button2Bit,Button0Port)

#define Button3Bit   3
#define Button3Port  PINC
#define IsDown3()    IsLow(Button3Bit,Button0Port)

#include <avr/io.h>
#include <avr/interrupt.h>
#include "global.h"
#include "timerx8.h"
#include "servo.h"
#include "uart.h"
#include "hservmot.c"


#define OUTPUT             1
#define INPUT              0


void steptimeInit(void) ;
void initUart(void) ;
void servoSetup(void) ;
void Delay(unsigned int delay) ;


// --| main |--

int main( void ) {

  unsigned char i;

  PORTC  = (1<<PC0)|(1<<PC1)|(1<<PC2)|(1<<PC3)|(1<<PC4)|(1<<PC5)|(1<<PC6);

  initMotors();   // initialize motor system
  initComms();    // initialize communication system
  initUart();     // initialize serial system
  timerInit();    // initialize timer system
  steptimeInit(); // initialize motion timer
  servoSetup();   // initialize servo system THIS MUST BE AFTER INITMOTORS
  sei();          // enable interrupts

  i = 0;
  while (1) {
    Delay(65535);
    Delay(65535);

    if (0) {
    } else if (IsDown0()) {  // next servo
      i++;
      if (i > 7) i = 0;
    } else if (IsDown1()) {  // servo to the right

        // dont let it roll over 255
       if ((255 - motors[i].position ) >= 4) {
         motors[i].target = motors[i].position + 4;
       } else {
         motors[i].target = 255;
       }
       SetBit(BEHAVIOUR_DIRRIGHT, motors[i].behaviour );
       ClearBit(BEHAVIOUR_SWEEP, motors[i].behaviour );

    } else if (IsDown2()) {  // servo to the left

       // dont let it roll under 0
       if (motors[i].position >= 4) {
         motors[i].target = motors[i].position - 4;
       } else {
         motors[i].target = 0;
       }
       ClearBit(BEHAVIOUR_DIRRIGHT, motors[i].behaviour );
       ClearBit(BEHAVIOUR_SWEEP, motors[i].behaviour );

    } else if (IsDown3()) {  // servo shake it all about
       motors[i].target = 128;
       ClearBit(BEHAVIOUR_SWEEP, motors[i].behaviour );

    }

  }
}


void Delay(unsigned int delay) {
  unsigned int x;
  for (x = delay; x != 0; x--) {
    asm volatile ("nop"::);
  }
}




void steptimeInit() {
 // set up timer for interrupt rate of about .001 sec (divide by 64 at 16Mhz)
 timer0SetPrescaler (TIMER_CLK_DIV64);
 // set timer to call doTick
 timerAttach ( TIMER0OVERFLOW_INT, doTick );
}

void initUart() {
  uartInit();                       // initialize UART (serial port)
  uartSetBaudRate(9600);            // set UART speed to 9600 baud
  uartSetRxHandler ( ReceiveByte );  // direct incomming data to our mouth
}

void servoSetup(void)   {
  unsigned char i;
  servoInit();
  servoSetChannelIO(0, _SFR_IO_ADDR(PORTB), PB0);
  servoSetChannelIO(1, _SFR_IO_ADDR(PORTB), PB1);
  servoSetChannelIO(2, _SFR_IO_ADDR(PORTB), PB2);
  servoSetChannelIO(3, _SFR_IO_ADDR(PORTB), PB3);
  servoSetChannelIO(4, _SFR_IO_ADDR(PORTB), PB4);
  servoSetChannelIO(5, _SFR_IO_ADDR(PORTB), PB5);

  DDRB =  (OUTPUT<<PB0)|(OUTPUT<<PB1)|(OUTPUT<<PB2)|(OUTPUT<<PB3)|
          (OUTPUT<<PB4)|(OUTPUT<<PB5);

  // enable and disable pullups
  PORTB  = (1<<PB0)|(1<<PB1)|(1<<PB2)|(1<<PB3)|(1<<PB4)|(1<<PB5);

  // make sure position is set on startup
  for (i = 0; i < 6; i++) {
   servoSetPosition(i, motors[i].position);
  }

}