/** * 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 main() { // Stop watchdog timer to prevent timeout reset WDTCTL = WDTPW + WDTHOLD; P1DIR = 0b00000011; // LED is P1.0, Trig is P1.1 P2DIR = 0b00110011; // LM is P2.4,P2.5 and RM is P2.0,P2.1 // This while loop repeats the four statements it contains // over and over again to blink the LED on and off forever while(1) { if (object_detect() == 1) { led(1); forward(); } else { led(0); spin_left(); } __delay_cycles(50000); // 50 ms delay to let echoes die down } }
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_left_turns(void) //sharp left 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 { spin_left(); //spins left (was spin right in v004) 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)//10100 { 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_left(); //spins left (was spin right in v004); check_sensors(); //constantly keeps checking to see LED combinations } } } //WE ADDED THIS LINE TO SEE IF IT MAKES A DIFFERENCE (ADDED IN v008) else if (SeeLine.b.Left&&SeeLine.b.CntLeft&&SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//11100 { 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_left(); //spins left (was spin right in v004) check_sensors(); //constantly keeps checking to see LED combinations } } } //THIS LINE FUNCTION ENDS HERE else if (SeeLine.b.Left&&SeeLine.b.CntLeft&&!SeeLine.b.Center&&!SeeLine.b.CntRight&&!SeeLine.b.Right)//11000 { 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_left(); //spins left (was spin right in v004); 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)//01000 { 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_left(); //spins left (was spin right in v004) 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_left(); //spins left (was spin right in v004) 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_left(); //spins left (was spin right in v004) 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(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_left(); //spins left (was spin right in v004) 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)//0000 { spin_left(); //spins left (was spin right in v004) 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 left_right_angle_turns(void) //left right angle turns, if else cases cover sharp turns aswell { straight_fwd(); //go straight 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)//00000 { spin_left(); //spins left (was spin right in v004) 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)//01000 { straight_fwd(); 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_left(); //spins left (was spin right in v004). 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_left(); //spins left (was spin right in v004). 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(); //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_left(); //spins left (was spin right in v004). 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(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_left(); //spins left (was spins right in v004); 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; } }