I am building a Johnny 5 inspired robot. I have a track assembly, a humanoid torso,Arduino Mega with a sensor shield, L298 Motor Driver Module,Torobot servo driver board, PS2 Controller, HC 05 Bluetooth module, SRF04 unit and 1.3" OLED display. I have upgraded the Scanning "Radar" project to have duplex communication, and have added modes, speeds and a tilt function for the head. I have an FPV cam, Video TX, IMU and an IR ranging module to add as well. I am using a Finite State Machine that is switched by the PS2 controller by using a combination of buttons to give me a maximum of 16 states that I can call from the remote. I have many pieces of code written for each of the states. State 2 is the scanning code. The backbone (PS2 controller and FSM) works great until I put in the scanning code. When I run this sketch I can enter state 2 but switching out of 2 it makes me wait until both halves of the scan are finished before changing states. Here is the code. What am I missing.
void poll_controller() {
ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
//ps2x.Analog(PSAB_BLUE)ps2x.Button(PSB_START)
if (ps2x.Analog(PSAB_GREEN)) { //will be TRUE as long as button is pressed
if (ps2x.NewButtonState()) //will be TRUE if any button changes state (on to off, or off to on)
{
if (ps2x.Button(PSB_L1))
//Serial.println("Tri+L1 pressed");
state = 1;
if (ps2x.Button(PSB_R1))
// Serial.println("Tri+R1 pressed");
state = 2;
if (ps2x.Button(PSB_L2))
// Serial.println("Tri+L2 pressed");
state = 3;
if (ps2x.Button(PSB_R2))
// Serial.println("Tri+R2 pressed");
state = 4;
}
}
if (ps2x.Analog(PSAB_RED)) { //will be TRUE as long as button is pressed
if (ps2x.NewButtonState()) //will be TRUE if any button changes state (on to off, or off to on)
{
if (ps2x.Button(PSB_L1))
state = 13;
//Serial.println("Cir+L1 pressed");
if (ps2x.Button(PSB_R1))
state = 14;
//Serial.println("Cir+R1 pressed");
if (ps2x.Button(PSB_L2))
state = 15;
//Serial.println("Cir+L2 pressed");
if (ps2x.Button(PSB_R2))
state = 16;
//Serial.println("Cir+R2 pressed");
}
}
}
void scanner() {
// Serial.println(state);
// rotates the servo motor from 15 to 165 degrees
for (int i = min_sweep; i <= max_sweep; i++) {
Pan_Servo.write(i);
//IMU_Data();
Tilt_Servo.write(j);
ps2x.read_gamepad(false, vibrate);
poll_controller();
delay(sweep_rate);
buttons();
get_US_range();
Serial.print(i); // Sends the current degree into the Serial Port
Serial.print(","); // Sends addition character right next to the previous value needed later in the Processing IDE for indexing
Serial.print(sonar.ping_result / US_ROUNDTRIP_CM); // Sends the distance value into the Serial Port
Serial.print("."); // Sends addition character right next to the previous value needed later in the Processing IDE for indexing
}
// Repeats the previous lines from 165 to 15 degrees
for (int i = max_sweep; i > min_sweep; i--) {
Pan_Servo.write(i);
//IMU_Data();
Tilt_Servo.write(j);
ps2x.read_gamepad(false, vibrate);
poll_controller();
delay(sweep_rate);
buttons();
get_US_range();
Serial.print(i);
Serial.print(",");
Serial.print(sonar.ping_result / US_ROUNDTRIP_CM);//Ranging data
Serial.print(".");
}
buttons();
ps2x.read_gamepad(false, vibrate);
poll_controller();
}