HuskyLens/Devastator basic code

Hello Fellow Backers,

I have managed to compile and test tethered basic Robot Devastator code, I still haven't got the untethered Bluetooth working yet, although the RomeoBLE RX does flash upon each command. I have the HuskyLens installed on a MG90 servo attached to port D3, but have not coded in, but display illuminates. As a start, Here is the code: and a link to DEMO video. ( https://youtu.be/c2Jpe7898S8 )

/*

# Editor : Phoebe

# Date : 2014.11.6

# Ver : 0.1

# Product: Devastator Tank Mobile Platform

# SKU : RBO0112

# Description:

# Connect the D4,D5,D6,D7,GND to UNO digital 4,5,6,7,GND

# Revised by James Martel

# Revision Date: 2019.12.29w

*/

int E1 = 5; //M1 Speed Control

int E2 = 6; //M2 Speed Control

int M1 = 4; //M1 Direction Control

int M2 = 7; //M1 Direction Control

void stop(void) //Stop

{

//digitalWrite(E1,0);

digitalWrite(E1,LOW);

//digitalWrite(E2,0);

digitalWrite(E2,LOW);

}

void advance(char a,char b) //Move forward

{

analogWrite (E1,a); //PWM Speed Control

digitalWrite(M1,HIGH);

analogWrite (E2,b);

digitalWrite(M2,HIGH);

}

void back_off (char a,char b) //Move backward

{

analogWrite (E1,a);

digitalWrite(M1,LOW);

analogWrite (E2,b);

digitalWrite(M2,LOW);

}

void turn_L (char a,char b) //Turn Left

{

analogWrite (E1,a);

digitalWrite(M1,LOW);

analogWrite (E2,b);

digitalWrite(M2,HIGH);

}

void turn_R (char a,char b) //Turn Right

{

analogWrite (E1,a);

digitalWrite(M1,HIGH);

analogWrite (E2,b);

digitalWrite(M2,LOW);

}

void setup(void)

{

int i;

for(i=4;i<=7;i++)

pinMode(i, OUTPUT);

Serial.begin(19200); //Set Baud Rate

Serial.println("Run keyboard control");

//digitalWrite(E1,LOW);

//digitalWrite(E2,LOW);

}

void loop(void)

{

if(Serial.available()){

char val = Serial.read();

if(val != -1)

{

switch(val)

{

case 'w'://Move Forward

advance (255,255); //move forward in max speed

break;

case 's'://Move Backward

back_off (255,255); //move back in max speed

break;

case 'a'://Turn Left

turn_L (200,150);

break;

case 'd'://Turn Right

turn_R (150,200);

break;

case 'z':

Serial.println("Hello");

break;

case 'x':

stop();

break;

}

}

else stop();

}

}