Sample Romeo code with Metro library, movement, and use of switch

The code is working with the Romeo controller (V1.0) and the RP5 chassis (while discontinued, it should work with Rover 5 or 2WD robot chassis as well). It builds on the basic DFRobot example by a) using switch 1 as an start/stop switch and b) instead of using delay(), it uses the Metro library.

The latter is because you don't want your robot to stop doing everything else, e.g., sensing, while moving, and with delay(), the controller stops doing everything with the exception of listening for interrupts.

When run, you need to push button 1 once the code is loaded. The robot then moves forward, turns, move forward, reverses, turns, and reverses again, with pauses between each movement. You can stop and restart the cycle by using switch 1.

I'm sure it could be coded better, but I thought it might be useful.
[code]
#include <Metro.h>

// Setup movement timer using the Metro library
Metro movementMetro = Metro(4000);

// Buttons vars

const int key_S1_5 = 7;

int state = false; // Off if 0, On if 1

int buttons_check(){
// This function checks button 1 and returns
// 0 if no button pressed, 1 if pressed
// Priorities: S1, S2, S3, S4, S5
int w = analogRead(key_S1_5);

#define vS2 130
if ( w < vS2/2 ){
return 1;
}
return 0;
}//End buttons_check()

// Set up for running motors
int E1 = 5;
int E2 = 6;
int M1 = 4;
int M2 = 7;

void forward(byte a,byte b) //Move forward
{
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}

void stop(void) //Stop
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}

void backward (byte a,byte b) //Move backward
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}


void Left (byte a,byte b) //Turn left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void Right (byte a,byte b) //Turn right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}
int movement = 1; // movement controls which movement (forward, turn, etc.)

void nextMovement (int next,unsigned long wait) // Set case and delay for next movement
{
if (movementMetro.check() == 1) {
movement = next;
movementMetro.interval(wait);
}
}

void setup(){
for(int i=4;i<=7;i++)
pinMode(i, OUTPUT);
Serial.begin(9600);
}// End setup
void loop(){

// This section checks button 1, which functions as stop/start switch, initially stopped
int button = buttons_check();
if ( button == 1 ) {
// Button 1 has been pressed
state = !state;
}
Serial.println(state);

// Whenstopped after running, reset to original start mode
if (state == false) {
stop(); //Stop
movement = 1;
movementMetro.interval(4000);
}
else {
Serial.println(movement);

// Each case sets movement command, signals next movement, and resets timer for duration OF NEXT MOVEMENT
switch (movement) {
case 1:
forward(255,255); //Forward at full speed
nextMovement(2, 5000);
break;
case 2:
stop(); //Stop
nextMovement(3, 650);
break;
case 3:
Right(255,255); //Right at full speed
nextMovement(4, 5000);
break;
case 4:
stop(); //Stop
nextMovement(5, 4000);
break;
case 5:
forward(255,255); //Forward at full speed
nextMovement(6, 5000);
break;
case 6:
stop(); //Stop
nextMovement(7, 4000);
break;
case 7:
backward(255,255); //backward at full speed
nextMovement(8, 5000);
break;
case 8:
stop(); //Stop
nextMovement(9, 650);
break;
case 9:
Left(255,255); //Left at full speed
nextMovement(10, 5000);
break;
case 10:
stop(); //Stop
nextMovement(11, 4000);
break;
case 11:
backward(255,255); //backward at full speed
nextMovement(12, 5000);
break;
case 12:
stop(); //Stop
nextMovement(1, 4000);
break;
}
}
}// End loop
[/code]