I connected Arduino Esplora to SSC-32 board made by Lynxmotion. I can use the thumbstick to move my robot. I think that there's a problem with the Arduino code. If I comment out the else parts of the code, the robot walks correctly. If I don't comment out the else parts of the code, there is a slight pause in the walking movement. How can I fix the problem?
#include <Esplora.h>
void setup()
{
Serial1.begin(9600);
robotstand01();
}
void loop()
{
if(Esplora.readButton(JOYSTICK_UP)==LOW)
{
robotwalk01();
delay(1000);
robotwalk02();
delay(1000);
robotwalk03();
delay(1000);
robotwalk04();
delay(1000);
robotwalk05();
delay(1000);
robotwalk06();
delay(1000);
}
//else
//{
// robotstand01();
// delay(1000);
//}
//delay(50);
if(Esplora.readButton(JOYSTICK_DOWN)==LOW)
{
robotwalk01();
delay(1000);
robotwalk02();
delay(1000);
robotwalk03();
delay(1000);
robotwalk04();
delay(1000);
robotwalk05();
delay(1000);
robotwalk06();
delay(1000);
}
//else
//{
// robotstand01();
// delay(1000);
//}
//delay(50);
if(Esplora.readButton(JOYSTICK_UP)==LOW || Esplora.readButton(JOYSTICK_DOWN)==LOW)
{
robotstand01();
delay(1000);
}
}
void move(int servo, int position, int time)
{
Serial1.print("#");
Serial1.print(servo);
Serial1.print(" P");
Serial1.print(position);
Serial1.print(" T");
Serial1.println(time);
}
void robotstand01()
{
move(0,1500,1000);
move(1,1500,1000);
move(2,1500,1000);
move(3,1500,1000);
move(4,1500,1000);
move(5,1500,1000);
move(16,1500,1000);
move(17,1500,1000);
move(18,1500,1000);
move(19,1500,1000);
move(20,1500,1000);
move(21,1500,1000);
}
void robotwalk01()
{
move(0,1167,1000);
move(1,1667,1000);
move(2,1833,1000);
move(3,1500,1000);
move(4,1167,1000);
move(5,1667,1000);
move(16,1167,1000);
move(17,1500,1000);
move(18,1833,1000);
move(19,1667,1000);
move(20,1167,1000);
move(21,1500,1000);
}
void robotwalk02()
{
move(0,1167,1000);
move(1,1500,1000);
move(2,1833,1000);
move(3,1500,1000);
move(4,1167,1000);
move(5,1500,1000);
move(16,1167,1000);
move(17,1500,1000);
move(18,1833,1000);
move(19,1500,1000);
move(20,1167,1000);
move(21,1500,1000);
}
void robotwalk03()
{
move(0,1167,1000);
move(1,1500,1000);
move(2,1833,1000);
move(3,1667,1000);
move(4,1167,1000);
move(5,1500,1000);
move(16,1167,1000);
move(17,1667,1000);
move(18,1833,1000);
move(19,1500,1000);
move(20,1167,1000);
move(21,1667,1000);
}
void robotwalk04()
{
move(0,1833,1000);
move(1,1500,1000);
move(2,1167,1000);
move(3,1667,1000);
move(4,1833,1000);
move(5,1500,1000);
move(16,1833,1000);
move(17,1667,1000);
move(18,1167,1000);
move(19,1500,1000);
move(20,1833,1000);
move(21,1667,1000);
}
void robotwalk05()
{
move(0,1833,1000);
move(1,1500,1000);
move(2,1167,1000);
move(3,1500,1000);
move(4,1833,1000);
move(5,1500,1000);
move(16,1833,1000);
move(17,1500,1000);
move(18,1167,1000);
move(19,1500,1000);
move(20,1833,1000);
move(21,1500,1000);
}
void robotwalk06()
{
move(0,1833,1000);
move(1,1667,1000);
move(2,1167,1000);
move(3,1500,1000);
move(4,1833,1000);
move(5,1667,1000);
move(16,1833,1000);
move(17,1500,1000);
move(18,1167,1000);
move(19,1667,1000);
move(20,1833,1000);
move(21,1500,1000);
}