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;
};
