void servoTest(void) { u08 pos; u08 channel; // do some examples // initialize RC servo system servoInit(); // setup servo output channel-to-I/Opin mapping // format is servoSetChannelIO( CHANNEL#, PORT, PIN ); servoSetChannelIO(0, _SFR_IO_ADDR(PORTC), PC0); servoSetChannelIO(1, _SFR_IO_ADDR(PORTC), PC1); servoSetChannelIO(2, _SFR_IO_ADDR(PORTC), PC2); servoSetChannelIO(3, _SFR_IO_ADDR(PORTC), PC3); // set port pins to output outb(DDRC, 0x0F); pos = 0; #define SPEED_SERVO 1 // spin servos sequentially back and forth between their limits while(1) { for(channel=0; channel<SERVO_NUM_CHANNELS; channel++) { for(pos=0; pos<SERVO_POSITION_MAX; pos++) { servoSetPosition(channel,pos); timerPause(SPEED_SERVO); } } for(channel=0; channel<SERVO_NUM_CHANNELS; channel++) { for(pos=SERVO_POSITION_MAX; pos>=1; pos--) { servoSetPosition(channel,pos); timerPause(SPEED_SERVO); } } } }
/** main() - where all the setup happens. */ int main(){ int RXbypass; // Bypass rx? unsigned char input, checksum; serialInit(115200); // Startup serial port // setup variables count = 0; servoSetPosition(PIN_SERVO, -80); // setup sensors gp2longInit(PIN_PAN_IR); gp2d12Init(PIN_LEFT_IR); gp2d12Init(PIN_RIGHT_IR); gp2d12Init(PIN_L_REAR_IR); gp2d12Init(PIN_R_REAR_IR); servoInit(PIN_SERVO,-15,1); // RX bypass enabled? digitalSetDirection(PIN_NO_RX, AVRRA_INPUT); digitalSetData(PIN_NO_RX, AVRRA_HIGH); if(digitalGetData(PIN_NO_RX) == AVRRA_LOW){ // bypass RX; RXbypass = TRUE; TXgo = TRUE; startFrame(); delayms(5000); }else{ RXbypass = FALSE; Print("MechDAR V1.0 Alive\n"); TXgo = FALSE; } // setup a 60Hz clock on Timer/Counter 2 TCNT2 = 0; OCR2A = 240; // this defines 60 Hz Clock TCCR2A = 0x03; // no A|B compare, mode7 fast pwm TCCR2B = 0x0F; // clock/1024 TIMSK2 |= 0x01; // interrupt on overflow // start clock sei(); while(1){ if((RXbypass == FALSE) && (serialAvailable() > 0)){ // process incoming if(serialRead() == -1){ // 0xFF while(serialAvailable() == 0); input = (unsigned char) serialRead(); while(serialAvailable() == 0); checksum = (unsigned char) serialRead(); PrintNumber(input); PrintNumber(checksum); // if a valid packet... if((255 - ((input + checksum)%256)) == 0){ // process the data switch(input){ case REG_LEFT_IR: PrintNumber(gp2d12GetData(PIN_LEFT_IR)); Print("\n"); break; case REG_RIGHT_IR: PrintNumber(gp2d12GetData(PIN_LEFT_IR)); Print("\n"); break; case REG_L_REAR_IR: PrintNumber(gp2d12GetData(PIN_LEFT_IR)); Print("\n"); break; case REG_R_REAR_IR: PrintNumber(gp2d12GetData(PIN_LEFT_IR)); Print("\n"); break; case REG_STOP: TXgo = FALSE; break; case REG_GO: TXgo = TRUE; startFrame(); break; default: // Send a reading from the data frame PrintNumber(data[(input-REG_PAN_BASE)%10]); Print("\n"); break; } } } } asm("nop"); } return 0; };