I have a little problem with a little code

Lynxmotion Legged Robots
1

hello,

[font=Times New Roman]I have written a small code for my hexapod[/font] ,

The config is:
Arduino Uno
ssc32 (ssc32 conect to Tx and Rx of the Arduino)
18 servo
Ps2 Controller

[font=Times New Roman]I have written 3[/font]	[font=Times New Roman]sequences: Init, AvanceTripod and InitFemurTibia controls with Ps2 : Start, Pad Up and Green Buttons.

The 3 sequences work correctly.
But for example, when i push " Pad Up" on Ps2 controller ([/font] [font=Times New Roman]sequence InitFemurTibia) the hexapod execute the 3 sequences (one by one).[/font] [font=Times New Roman]He should just execute the sequence corresponding to the button, then wait for a new " order "[/font].
[font=Times New Roman]I suppose that there is an error in my code or syntax[/font] …
Can you help me or have you suggestion ?
Sorry for my English i’m rusty :smiley:

Here my code:

[code]//Programme principale Hexo_Bot Ver1.02
//Ecrit par Alain Buchard (Dracub) Septembre 2014
#if ARDUINO>99
#include <Arduino.h>
#else
#endif
#include <PS2X_lib.h>
#include <pins_arduino.h>
#include <SoftwareSerial.h>

#define SOUND_PIN 5 // buzzer

PS2X ps2x;

int move;
int error = 0; //default…
int pos = 0; // variable to store the servo position //copy from servo code
byte type = 0;
byte vibrate = 0;

void Init(){
//sequence d’initialisation
//Coxas 1,2,3,4,5,6 en position centre (Init)
Serial.println("#24P1500#20P1420#16P1500#8P1500#4P1420#0P1500T500");
delay(1550);
//Femurs 1,2,3,4,5,6 en position centre (Init)
Serial.println("#25P1420#21P1420#17P1550#9P1565#5P1400#1P1450T500");
delay(1550);
//Tibias 1,2,3,4,5,6 en position centre (Init)
Serial.println("#26P1330#22P1470#18P1470#10P1375#6P1470#2P1500T500");
delay(80);
}

void AvanceTripod(){
//sequence marche avant “tripod”
//Femurs 1,3,5 Init-300 PWM (HAUT) T800
Serial.println("#25P1120#17P1250#5P1100T800");
delay(1700);
//[Coxas 1,3 Init+150 et 5 Init-150 (vers l’AVANT)] et [Coxas 2 Init-150 et 4,6 Init+150 (vers l’ARRIERE)] T500
Serial.println("#24P1650#16P1650#4P1270#20P1270#8P1650#0P1650T500");
delay(1200);
//Femurs 1,3,5 Init (CENTRE) T800
Serial.println("#25P1420#17P1550#5P1400T800");
delay(1200);
//Femurs 2,4,6 Init-300 PWM T800
Serial.println("#21P1120#9P1265#1P1150T800");
delay(1700);
//Coxas 1,3,5 Init et Coxas 2 Init+150 et 4,6 Init-150 T500
Serial.println("#24P1500#16P1500#4P1420#20P1570#8P1350#0P1350T500");
delay(1200);
//Femurs 2,4,6 Init T800
Serial.println("#21P1420#9P1565#1P1450T800");
delay(1700);
}

void InitFemurTibia(){
//Tibia 1 Init-500 et Femur 1 Init-500 “up” T400
Serial.println("#26P1830#25P920T400");
delay(500);
Serial.println("#26P1330#25P1420T400");//Tibia 1 et Femur 1 Init
delay(500);

//Tibia 2 Init-500 et Femur 2 Init-500 “up” T400
Serial.println("#22P1970#21P920T400");
delay(500);
Serial.println("#22P1470#21P1420T400");//Tibia 2 et Femur 2 Init
delay(500);

//Tibia 3 Init-500 et Femur 3 Init-500 “up” T400
Serial.println("#18P1970#17P1050T400");
delay(500);
Serial.println("#18P1470#17P1550T400");//Tibia 3 et Femur 3 Init
delay(500);

//Tibia 4 Init-500 et Femur 4 Init-500 “up” T400
Serial.println("#10P1875#9P1065T400");
delay(500);
Serial.println("#10P1375#9P1565T400");//Tibia 4 et Femur 4 Init
delay(500);

//Tibia 5 Init-500 et Femur 5 Init-500 “up” T400
Serial.println("#6P1970#5P900T400");
delay(500);
Serial.println("#6P1470#5P1400T400");//Tibia 5 et Femur 5 Init
delay(500);

//Tibia 6 Init-500 et Femur 6 Init-500 “up” T400
Serial.println("#2P2000#1P950T400");
delay(500);
Serial.println("#2P1500#1P1450T400");//Tibia 6 et Femur 6 Init
delay(500);

}

void setup(){
Serial.begin(38400); //transmission rate between controller and motors

error = ps2x.config_gamepad(13,11,10,12, true, true); //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error

}

void loop(){
if(error == 1) //skip loop if no controller found
return;

//Check values of controller (check state)
ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at ‘vibrate’ speed

//position Init
if(ps2x.ButtonPressed(PSB_START)){
Init();
}

//Avance Tripod_Gait
if(ps2x.ButtonPressed(PSB_PAD_UP)){
AvanceTripod();
}

//Sequence InitFemurTibia
if(ps2x.ButtonPressed(PSB_GREEN)){
InitFemurTibia();
}

delay(200);
}

[/code]

2

Everything seems okay for me… Maybe if you try adding a “return;” inside each of the three “if” statements? This will make the loop restart and read the gamepad again…

3

ok jarcand

I’ll try tomorrow and post the result on the forum

4

I have solved the problem with: goto ;

exemple:

[code] void loop(){
sortir:
if(error == 1) //skip loop if no controller found
return;

//Check values of controller (check state)
ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at ‘vibrate’ speed

//position Init
if(ps2x.ButtonPressed(PSB_START)){
Init();
goto sortir;
}

//Avance Tripod_Gait
if(ps2x.ButtonPressed(PSB_PAD_UP)){
AvanceTripod();
goto sortir;
}

//Sequence InitFemurTibia
if(ps2x.ButtonPressed(PSB_GREEN)){
InitFemurTibia();
goto sortir;
}

delay(200);
}

[/code]