void interruptions_left(void) //for interruptions appearing on the left to the track
{
straight_fwd(); // Go forward
Delay10KTCYx(130);
check_sensors();
if (SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //10100
{
while(SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000 sees nothing
{ //spin right
spin_right();
check_sensors();
}
}
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&SeeLine.b.Right) //00101
{
straight_fwd(); // Go forward
Delay10KTCYx(130);
check_sensors();
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
spin_right();
check_sensors();
}
}
}
*
}
void main(void) {
WDTCTL = WDTPW + WDTHOLD; //disable watchdog
//initialization stuff here
//remember to enable any pull-up/downs!
P1OUT = 0;
P1DIR |= ALL_MOTORS;
P1SEL |= MOTOR_LR_PWM;
P1SEL2 |= MOTOR_L_B; //P1SEL2 needed as well for TA0.2
BCSCTL1 = CALBC1_1MHZ; // Running at 1 MHz
DCOCTL = CALDCO_1MHZ;
TA_init();
//_BIS_SR(LPM0_bits + GIE); // Enter LPM0 and enable interrupts
//infinite loop
for(;;) {
int spindelay = 2000; //should be in a #define
int fwddelay = 4000; //but was too lazy.
go_forwards(1000); //100%
bad_delay_ms(fwddelay);
spin_right(1000);
bad_delay_ms(spindelay);
go_forwards(750); //75%
bad_delay_ms(fwddelay);
spin_right(1000);
bad_delay_ms(spindelay);
go_forwards(500); //50%
bad_delay_ms(fwddelay);
spin_right(1000);
bad_delay_ms(spindelay);
go_forwards(250); //25%
bad_delay_ms(fwddelay);
spin_right(1000);
bad_delay_ms(spindelay);
/*for (i = 0; i < 1000;i+=200){
//TACCR1 = i;
//TACCR2 = i;
//set_motor_speed(0,i);
//set_motor_speed(1,i);
set_motor_speeds(i,i);
bad_delay_ms(2000);
//__delay_cycles(100000);
}*/
}
}
/**
* Function main
* @author Danny
* @date 2017.08.16
* @brief delay 2s，advance 1s，back 1s,turn left 2s,turn right 2s,
* turn left in place 3s, turn right in place 3s,stop 0.5s
* @param[in] void
* @retval void
* @par History
*/
void main()
{
wiringPiSetup();
//Initializethe motor drive IO as the output mode
pinMode(Left_motor_go, OUTPUT);
pinMode(Left_motor_back, OUTPUT);
pinMode(Right_motor_go, OUTPUT);
pinMode(Right_motor_back, OUTPUT);
//int softPwmCreate(int pin,int initialValue,int pwmRange);
softPwmCreate(Left_motor_pwm,0,255);
softPwmCreate(Right_motor_pwm,0,255);
while(1)
{
delay(2000);
run(10);
back(10);
left(20);
right(20);
spin_left(30);
spin_right(30);
brake(5);
}
return;
}
void spin_random(){
if(random(1,10)>5){
spin_right();
}else{
spin_left();
}
}
void go_straight(void)
{
// very simple motor control
if ( SeeLine.b.Center ) straight_fwd();
else if (SeeLine.b.Left) spin_left();
else if (SeeLine.b.CntLeft) turn_left();
else if (SeeLine.b.CntRight) turn_right();
else if (SeeLine.b.Right) spin_right();
if ( (SeeLine.B ) == 0b00000u) motors_brake_all();
}
void sharp_turns(void)
{
if( (SeeLine.B)==0b00100u)
{
if( (SeeLine.B)==0b10100u)
spin_left();
}
else if( (SeeLine.B)==0b00100u)
{
if( (SeeLine.B)==0b00101u)
spin_right();
}
else
straight_fwd();
}
void sharp_right_turns(void) //sharp right turns
{
straight_fwd(); //go straight
Delay10KTCYx(130); //Changed from Delay10KTCYx(50); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000 sees nothing
{
spin_right(); //spins right
check_sensors(); //constantly keeps checking to see LED combinations
}
}
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&SeeLine.b.Right) //00101
{
straight_fwd(); //go straight
Delay10KTCYx(130); //Changed from Delay10KTCYx(50); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations.
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
spin_right(); //spins right
check_sensors(); //constantly keeps checking to see LED combinations.
}
}
}
//THIS WAS ADDED IN v008
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&SeeLine.b.CntRight&&SeeLine.b.Right)//00111
{
straight_fwd(); //go straight
Delay10KTCYx(130); //Changed from Delay10KTCYx(50); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
spin_right(); //spins right
check_sensors(); //constantly keeps checking to see LED combinations
}
}
}
//FUNCTION STOPS HERE
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&SeeLine.b.CntRight&&SeeLine.b.Right) //00011
{
straight_fwd(); //go straight
Delay10KTCYx(130); //Changed from Delay10KTCYx(50); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
spin_right(); //spins right
check_sensors(); //constantly keeps checking to see LED combinations
}
}
}
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&SeeLine.b.CntRight&&!SeeLine.b.Right)//00010
{
straight_fwd(); //go straight
Delay10KTCYx(130); //Changed from Delay10KTCYx(50); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
spin_right(); //spins right
check_sensors(); //constantly keeps checking to see LED combinations.
}
}
}
else if (!SeeLine.b.Left&&SeeLine.b.CntLeft&&SeeLine.b.Center&&SeeLine.b.CntRight&&!SeeLine.b.Right)//01110
{
straight_fwd(); //go straight
Delay10KTCYx(130); //Changed from Delay10KTCYx(50); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations.
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
spin_right(); //spins right
check_sensors(); //constantly keeps checking to see LED combinations
}
}
}
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00100
{
straight_fwd(); //go straight
Delay10KTCYx(130); //Changed from Delay10KTCYx(50); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
spin_right(); //spins right
check_sensors(); //constantly keeps checking to see LED combinations
}
}
}
else if (!SeeLine.b.Left&&SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//01100
{
straight_fwd(); //go straight
Delay10KTCYx(130); //Changed from Delay10KTCYx(50); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
spin_right(); //spins right
check_sensors(); //constanlty keeps checking to see LED combinations
}
}
}
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&SeeLine.b.CntRight&&!SeeLine.b.Right)//00110
{
straight_fwd(); //go straight
Delay10KTCYx(130); //Changed from Delay10KTCYx(50); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations.
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//00000
{
spin_right(); //spins right
check_sensors(); //constantly keeps checking to see LED combinations.
}
}
}
else;
}
void motor_control(void) //Different scenerios that will light up the LEDs in a certain way,
//and the motor reactions to the scenarios.
{
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) straight_fwd(); //00100
else if (SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) spin_left(); //10000
else if (SeeLine.b.Left&&SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) spin_left(); //11000
else if (!SeeLine.b.Left&&SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) turn_left(); //01000
else if (!SeeLine.b.Left&&SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) turn_left(); //01100
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&SeeLine.b.CntRight&&!SeeLine.b.Right) turn_right();//00010
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&SeeLine.b.CntRight&&!SeeLine.b.Right) turn_right(); //00110
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&SeeLine.b.Right) spin_right();//00001
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&SeeLine.b.CntRight&&SeeLine.b.Right) spin_right(); //00011
else if ( (SeeLine.B ) == 0b00000u) Check_Gaps(); //00000 //all LEDs OFF
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&SeeLine.b.CntRight&&SeeLine.b.Right) right_right_angle_turns(); //00111
else if (SeeLine.b.Left&&SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) left_right_angle_turns(); //11100
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&SeeLine.b.Right) sharp_right_turns(); //00101
else if (SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) sharp_left_turns(); //10100
else if (SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&SeeLine.b.CntRight&&!SeeLine.b.Right) merge_right(); //10010
else if (!SeeLine.b.Left&&SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&SeeLine.b.Right) merge_left(); //01001
else if (SeeLine.b.Left&&SeeLine.b.CntLeft&&!SeeLine.b.Center&&SeeLine.b.CntRight&&!SeeLine.b.Right) merge_right(); //11010
else if (!SeeLine.b.Left&&SeeLine.b.CntLeft&&!SeeLine.b.Center&&SeeLine.b.CntRight&&SeeLine.b.Right) merge_left(); //01011
else if (SeeLine.b.Left&&SeeLine.b.CntLeft&&SeeLine.b.Center&&SeeLine.b.CntRight&&SeeLine.b.Right) final_ends(); //11111 //all LEDs ON
}
void right_right_angle_turns(void) //right right angle turns, if-else cases cover sharp turns as well
{
straight_fwd();
Delay10KTCYx(50); //Changed from Delay10KTCYx(25); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations.
if ( (SeeLine.B ) == 0b00000u) //all LEDs OFF.
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //while all off; 00000
{
spin_right(); //spins the robot right.
check_sensors(); //constantly keeps checking to see LED combinations.
}
}
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&SeeLine.b.CntRight&&!SeeLine.b.Right) //00010
{
straight_fwd(); //go straight
Delay10KTCYx(50); //Changed from Delay10KTCYx(25); (v 009)
check_sensors();
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
spin_right(); //spins the robot right.
check_sensors(); //constantly keeps checking to see LED combinations.
}
}
}
else if (!SeeLine.b.Left&&SeeLine.b.CntLeft&&SeeLine.b.Center&&SeeLine.b.CntRight&&!SeeLine.b.Right) //01110
{
straight_fwd(); //Go straight.
Delay10KTCYx(50); //Changed from Delay10KTCYx(25); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations.
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
spin_right(); //Rotates Right
check_sensors(); //constantly keeps checking to see LED combinations.
}
}
}
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00100
{
straight_fwd(); //Go straight.
Delay10KTCYx(50); //Changed from Delay10KTCYx(25); (v 009)
check_sensors(); //contantly keeps checking to see LED combinations.
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
spin_right(); //spins right
check_sensors(); //constantly keeps checking to see LED combinations.
}
}
}
else if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&SeeLine.b.Center&&SeeLine.b.CntRight&&!SeeLine.b.Right) //00110
{
straight_fwd(); //Go straight.
Delay10KTCYx(50); //Changed from Delay10KTCYx(25); (v 009)
check_sensors(); //constantly keeps checking to see LED combinations.
if (!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
while(!SeeLine.b.Left&&!SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right) //00000
{
spin_right(); //Rotates right.
check_sensors(); //constantly keeps checking to see LED combinations.
}
}
}
else;
}
/**
* Function main
* @author Danny
* @date 2017.08.16
* @brief When key is pressed, Ultrasonic obstacle avoidance mode is opened
* @param[in] void
* @retval void
* @par History
*/
void main()
{
float distance;
//Initialize wiringPi
wiringPiSetup();
//Initialize the motor drive IO as the output mode
pinMode(Left_motor_go, OUTPUT);
pinMode(Left_motor_back, OUTPUT);
pinMode(Right_motor_go, OUTPUT);
pinMode(Right_motor_back, OUTPUT);
softPwmCreate(Left_motor_pwm,0,255);
softPwmCreate(Right_motor_pwm,0,255);
//Initialize the key interface as the input mode
pinMode(key, INPUT);
//Initialize ultrasonic pin
pinMode(EchoPin, INPUT);
pinMode(TrigPin, OUTPUT);
key_scan();
while(1)
{
distance = Distance_test();
if (distance > 55)
{
run(150, 150);
}
else if (distance >= 30 && distance <= 55)
{
run(80, 80);
}
else if (distance < 30)
{
spin_right(350);
brake(1);
distance = Distance_test();
if (distance >= 30)
{
run(120, 120);
}
else if (distance < 30)
{
spin_left(800);
brake(1);
distance = Distance_test();
if (distance >= 30)
{
run(120, 120);
}
else if (distance < 30)
{
spin_left(400);
brake(1);
}
}
}
}
return;
}
/**
* Function main
* @author Danny
* @date 2017.08.16
* @brief When key is pressed, Ultrasonic obstacle avoidance mode is opened
* @param[in] void
* @retval void
* @par History
*/
void main()
{
//Initialize wiringPi
wiringPiSetup();
//Initialize the motor drive IO as the output mode
pinMode(Left_motor_go, OUTPUT);
pinMode(Left_motor_back, OUTPUT);
pinMode(Right_motor_go, OUTPUT);
pinMode(Right_motor_back, OUTPUT);
//Create two software-controlled PWM pins to control motor speed
softPwmCreate(Left_motor_pwm,0,255);
softPwmCreate(Right_motor_pwm,0,255);
//Initialize the key interface as the input mode
pinMode(key, INPUT);
//Initialize the tracksensor IO as the input mode
pinMode(TrackSensorLeftPin1, INPUT);
pinMode(TrackSensorLeftPin2, INPUT);
pinMode(TrackSensorRightPin1, INPUT);
pinMode(TrackSensorRightPin2, INPUT);
key_scan();
while(1)
{
// When the black line is detected, the corresponding indicator of the tracking module is on, and the port level is LOW.
// When the black line is not detected, the corresponding indicator of the tracking module is off, and the port level is HIGH.
TrackSensorLeftValue1 = digitalRead(TrackSensorLeftPin1);
TrackSensorLeftValue2 = digitalRead(TrackSensorLeftPin2);
TrackSensorRightValue1 = digitalRead(TrackSensorRightPin1);
TrackSensorRightValue2 = digitalRead(TrackSensorRightPin2);
//4 tracking pins level status
// 0 0 X 0
// 1 0 X 0
// 0 1 X 0
//Turn right in place,speed is 150,delay 80ms
//Handle right acute angle and right right angle
if ( (TrackSensorLeftValue1 == LOW || TrackSensorLeftValue2 == LOW) && TrackSensorRightValue2 == LOW)
{
spin_right(150, 150);
delay(80);
}
//4 tracking pins level status
// 0 X 0 0
// 0 X 0 1
// 0 X 1 0
//Turn right in place,speed is 150,delay 80ms
//Handle left acute angle and left right angle
else if ( TrackSensorLeftValue1 == LOW && (TrackSensorRightValue1 == LOW || TrackSensorRightValue2 == LOW))
{
spin_left(150, 150);
delay(80);
}
// 0 X X X
//Left_sensor1 detected black line
else if ( TrackSensorLeftValue1 == LOW)
{
spin_left(150, 150);
// delay(10);
}
// X X X 0
//Right_sensor2 detected black line
else if ( TrackSensorRightValue2 == LOW )
{
spin_right(150, 150);
// delay(10);
}
//4 tracking pins level status
// X 0 1 X
else if ( TrackSensorLeftValue2 == LOW && TrackSensorRightValue1 == HIGH)
{
left(0, 150);
}
//4 tracking pins level status
// X 1 0 X
else if (TrackSensorLeftValue2 == HIGH && TrackSensorRightValue1 == LOW)
{
right(150, 0);
}
//4 tracking pins level status
// X 0 0 X
else if (TrackSensorLeftValue2 == LOW && TrackSensorRightValue1 == LOW)
{
run(150, 150);
}
//When the level of 4 pins are 1 1 1 1 , the car keeps the previous running state.
}
return;
}
/**
* commandParser parse commands received by the twi interface and trigger the
* associated functions
*/
void parse_cmd(uint8_t cmd[CMD_SIZE])
{
uint8_t i;
/* Check new conditions and update status from tuxaudio */
if (cmd[0] == SEND_AUDIOSENSORS_CMD)
{
if ((cmd[1] & STATUS_HEADBTN_MK)
&& !(gStatus.sw & GSTATUS_HEADBTN_MK))
cond_flags.head = 1;
if ((cmd[1] & STATUS_LEFTWINGBTN_MK)
&& !(gStatus.sw & GSTATUS_LEFTWINGBTN_MK))
cond_flags.left_flip = 1;
if ((cmd[1] & STATUS_RIGHTWINGBTN_MK)
&& !(gStatus.sw & GSTATUS_RIGHTWINGBTN_MK))
cond_flags.right_flip = 1;
if ((cmd[1] & STATUS_CHARGER_MK)
&& !(gStatus.sw & GSTATUS_CHARGER_MK))
cond_flags.charger_start = 1;
if (!(cmd[1] & STATUS_POWERPLUGSW_MK)
&& (gStatus.sw & GSTATUS_POWERPLUGSW_MK))
cond_flags.unplug = 1;
if ((cmd[1] & STATUS_RF_MK) && !(gStatus.sw & GSTATUS_RF_MK))
{
cond_flags.rf_conn = 1;
cond_flags.rf_disconn = 0;
}
if (!(cmd[1] & STATUS_RF_MK) && (gStatus.sw & GSTATUS_RF_MK))
{
cond_flags.rf_conn = 0;
cond_flags.rf_disconn = 1;
}
gStatus.sw = cmd[1];
gStatus.audio_play = cmd[2];
gStatus.audio_status = cmd[3];
return;
}
/* Ping */
else if (cmd[0] == PING_CMD)
{
pingCnt = cmd[1];
return;
}
/* Sound */
else if (cmd[0] == PLAY_SOUND_CMD)
{
/* Forward the cmd to the audio CPU. */
queue_cmd(cmd);
return;
}
else if (cmd[0] == MUTE_CMD)
{
/* Forward the cmd to the audio CPU. */
queue_cmd(cmd);
return;
}
/* Sleep mode */
else if (cmd[0] == SLEEP_CMD)
{
cond_flags.sleep = true;
return;
}
/* Version */
else if (cmd[0] == INFO_TUXCORE_CMD)
{
uint8_t *p = (uint8_t *) &tag_version;
uint8_t info[12];
for (i = 0; i < 12; i++)
info[i] = pgm_read_byte(p++);
queue_cmd(&info[0]);
queue_cmd(&info[4]);
queue_cmd(&info[8]);
return;
}
/* Reset condition flags */
else if (cmd[0] == COND_RESET_CMD)
{
uint8_t *addr = (uint8_t *) & cond_flags;
for (i = 0; i < COND_RESET_NBR; i++)
*addr++ = 0;
return;
}
else if (cmd[0] == LED_FADE_SPEED_CMD)
{
led_set_fade_speed(cmd[1], cmd[2], cmd[3]);
}
else if (cmd[0] == LED_SET_CMD)
{
led_set_intensity(cmd[1], cmd[2]);
}
else if (cmd[0] == IR_SEND_RC5_CMD)
{
irSendRC5(cmd[1], cmd[2]);
}
else if (cmd[0] == MOTORS_CONFIG_CMD)
{
motors_config(cmd[1], cmd[2]);
}
/* Leds */
else if (cmd[0] == LED_PULSE_RANGE_CMD)
{
led_pulse_range(cmd[1], cmd[2], cmd[3]);
}
/* Move */
else
{
if (cmd[0] == MOTORS_SET_CMD)
{
motors_run(cmd[1], cmd[2], cmd[3]);
}
else if (cmd[0] == LED_PULSE_CMD)
{
led_pulse(cmd[1], cmd[2], cmd[3]);
}
/* Deprecated functions, though they can be kept for the standalone as
* they're simpler than the other LED functions. */
else if (cmd[0] == LED_ON_CMD)
{
led_set_intensity(LED_BOTH, 0xFF);
}
else if (cmd[0] == LED_OFF_CMD)
{
led_set_intensity(LED_BOTH, 0x0);
}
else if (cmd[0] == LED_TOGGLE_CMD)
{
leds_toggle(cmd[1], cmd[2]);
}
/* Moves */
else if (cmd[0] == BLINK_EYES_CMD)
{
blink_eyes(cmd[1]);
}
else if (cmd[0] == STOP_EYES_CMD)
{
stop_eyes();
}
else if (cmd[0] == OPEN_EYES_CMD)
{
open_eyes();
}
else if (cmd[0] == CLOSE_EYES_CMD)
{
close_eyes();
}
else if (cmd[0] == MOVE_MOUTH_CMD)
{
move_mouth(cmd[1]);
}
else if (cmd[0] == OPEN_MOUTH_CMD)
{
open_mouth();
}
else if (cmd[0] == CLOSE_MOUTH_CMD)
{
close_mouth();
}
else if (cmd[0] == STOP_MOUTH_CMD)
{
stop_mouth();
}
else if (cmd[0] == WAVE_WINGS_CMD)
{
wave_flippers(cmd[1], cmd[2]);
}
else if (cmd[0] == RAISE_WINGS_CMD)
{
raise_flippers();
}
else if (cmd[0] == LOWER_WINGS_CMD)
{
lower_flippers();
}
else if (cmd[0] == RESET_WINGS_CMD)
{
reset_flippers();
}
else if (cmd[0] == STOP_WINGS_CMD)
{
stop_flippers();
}
else if (cmd[0] == SPIN_LEFT_CMD)
{
spin_left(cmd[1], cmd[2]);
}
else if (cmd[0] == SPIN_RIGHT_CMD)
{
spin_right(cmd[1], cmd[2]);
}
else if (cmd[0] == STOP_SPIN_CMD)
{
stop_spinning();
}
/* Undefined commands */
else
return; /* simply drop it */
/* Send an updated status here for functions that need it */
updateStatusFlag = 1;
}
}
