Cars

#include <LiquidCrystal_I2C.h>
#include <Wire.h>
LiquidCrystal_I2C lcd(0x27,16,2);

// RemoteXY select connection mode and include library
#define REMOTEXY_MODE__HARDSERIAL

#include <RemoteXY.h>

// RemoteXY connection settings
#define REMOTEXY_SERIAL Serial
#define REMOTEXY_SERIAL_SPEED 9600

// RemoteXY configurate
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =
  { 255,6,0,0,0,112,0,10,120,0,
  2,1,4,3,18,9,1,26,31,31,
  78,121,97,108,97,104,0,77,97,116,
  105,0,5,20,71,35,24,24,1,24,
  31,2,1,4,50,18,9,1,26,31,
  31,76,97,109,112,117,32,110,121,97,
  108,97,104,0,76,97,109,112,117,32,
  109,97,116,105,0,2,1,4,36,18,
  9,1,26,31,31,83,105,114,101,110,
  101,32,110,121,97,108,97,104,0,83,
  105,114,101,110,101,32,109,97,116,105,
  0,3,134,58,5,36,7,1,26 };

// this structure defines all the variables and events of your control interface
struct {

    // input variables
  uint8_t saklar; // =1 if switch ON and =0 if OFF
  int8_t joystick_1_x; // =-100..100 x-coordinate joystick position
  int8_t joystick_1_y; // =-100..100 y-coordinate joystick position
  uint8_t lampu; // =1 if switch ON and =0 if OFF
  uint8_t sirene; // =1 if switch ON and =0 if OFF
  uint8_t speed_motor; // =0 if select position A, =1 if position B, =2 if position C, ...

    // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0

} RemoteXY;
#pragma pack(pop)

/* defined the right motor control pins */
#define PIN_MOTOR_RIGHT_UP 7
#define PIN_MOTOR_RIGHT_DN 6
#define PIN_MOTOR_RIGHT_SPEED 10

/* defined the left motor control pins */
#define PIN_MOTOR_LEFT_UP 5
#define PIN_MOTOR_LEFT_DN 4
#define PIN_MOTOR_LEFT_SPEED 9

#define PIN_SAKLAR 13
#define PIN_LAMPU 11
#define PIN_SIRENE 12

/* defined two arrays with a list of pins for each motor */
unsigned char RightMotor[3] =
  {PIN_MOTOR_RIGHT_UP, PIN_MOTOR_RIGHT_DN, PIN_MOTOR_RIGHT_SPEED};
unsigned char LeftMotor[3] =
  {PIN_MOTOR_LEFT_UP, PIN_MOTOR_LEFT_DN, PIN_MOTOR_LEFT_SPEED};

/*
   speed control of the motor
   motor - pointer to an array of pins
   v - motor speed can be set from -100 to 100
*/

void Sleep_speed (unsigned char * motor1, int v1)
{
  if (v1>100) v1=100;
  if (v1<-100) v1=-100;
  if (v1>0) {
    digitalWrite(motor1[0], HIGH);
    digitalWrite(motor1[1], LOW);
    analogWrite(motor1[2], v1*0);
  }
  else if (v1<0) {
    digitalWrite(motor1[0], LOW);
    digitalWrite(motor1[1], HIGH);
    analogWrite(motor1[2], (-v1)*0);
  }
  else {
    digitalWrite(motor1[0], LOW);
    digitalWrite(motor1[1], LOW);
    analogWrite(motor1[2], 0);
  }
}

void Wheel1 (unsigned char * motor2, int v2)
{
  if (v2>100) v2=100;
  if (v2<-100) v2=-100;
  if (v2>0) {
    digitalWrite(motor2[0], HIGH);
    digitalWrite(motor2[1], LOW);
    analogWrite(motor2[2], v2*0.51);
  }
  else if (v2<0) {
    digitalWrite(motor2[0], LOW);
    digitalWrite(motor2[1], HIGH);
    analogWrite(motor2[2], (-v2)*0.51);
  }
  else {
    digitalWrite(motor2[0], LOW);
    digitalWrite(motor2[1], LOW);
    analogWrite(motor2[2], 0);
  }
}

void Wheel2 (unsigned char * motor3, int v3)
{
  if (v3>100) v3=100;
  if (v3<-100) v3=-100;
  if (v3>0) {
    digitalWrite(motor3[0], HIGH);
    digitalWrite(motor3[1], LOW);
    analogWrite(motor3[2], v3*1.02);
  }
  else if (v3<0) {
    digitalWrite(motor3[0], LOW);
    digitalWrite(motor3[1], HIGH);
    analogWrite(motor3[2], (-v3)*1.02);
  }
  else {
    digitalWrite(motor3[0], LOW);
    digitalWrite(motor3[1], LOW);
    analogWrite(motor3[2], 0);
  }
}

void Wheel3 (unsigned char * motor4, int v4)
{
  if (v4>100) v4=100;
  if (v4<-100) v4=-100;
  if (v4>0) {
    digitalWrite(motor4[0], HIGH);
    digitalWrite(motor4[1], LOW);
    analogWrite(motor4[2], v4*1.53);
  }
  else if (v4<0) {
    digitalWrite(motor4[0], LOW);
    digitalWrite(motor4[1], HIGH);
    analogWrite(motor4[2], (-v4)*1.53);
  }
  else {
    digitalWrite(motor4[0], LOW);
    digitalWrite(motor4[1], LOW);
    analogWrite(motor4[2], 0);
  }
}

void Wheel4 (unsigned char * motor5, int v5)
{
  if (v5>100) v5=100;
  if (v5<-100) v5=-100;
  if (v5>0) {
    digitalWrite(motor5[0], HIGH);
    digitalWrite(motor5[1], LOW);
    analogWrite(motor5[2], v5*2.04);
  }
  else if (v5<0) {
    digitalWrite(motor5[0], LOW);
    digitalWrite(motor5[1], HIGH);
    analogWrite(motor5[2], (-v5)*2.04);
  }
  else {
    digitalWrite(motor5[0], LOW);
    digitalWrite(motor5[1], LOW);
    analogWrite(motor5[2], 0);
  }
}

void Wheel5 (unsigned char * motor6, int v6)
{
  if (v6>100) v6=100;
  if (v6<-100) v6=-100;
  if (v6>0) {
    digitalWrite(motor6[0], HIGH);
    digitalWrite(motor6[1], LOW);
    analogWrite(motor6[2], v6*2.55);
  }
  else if (v6<0) {
    digitalWrite(motor6[0], LOW);
    digitalWrite(motor6[1], HIGH);
    analogWrite(motor6[2], (-v6)*2.55);
  }
  else {
    digitalWrite(motor6[0], LOW);
    digitalWrite(motor6[1], LOW);
    analogWrite(motor6[2], 0);
  }
}

void setup () {
   lcd.begin();
   lcd.backlight();
   Serial.begin(115200);

   lcd.setCursor(0,0);
   lcd.print("ON");
   delay(3000);
   lcd.clear();

  pinMode (PIN_MOTOR_RIGHT_UP, OUTPUT);
  pinMode (PIN_MOTOR_RIGHT_DN, OUTPUT);
  pinMode (PIN_MOTOR_LEFT_UP, OUTPUT);
  pinMode (PIN_MOTOR_LEFT_DN, OUTPUT);

  pinMode (PIN_SAKLAR, OUTPUT);
  pinMode (PIN_LAMPU, OUTPUT);
  pinMode (PIN_SIRENE, OUTPUT);

  /* initialization module RemoteXY */
  RemoteXY_Init ();
}

void loop () {

        RemoteXY_Handler ();
        if (RemoteXY.saklar!=0) {

         if (RemoteXY.speed_motor==0) {
           Sleep_speed (RightMotor, RemoteXY.joystick_1_y - RemoteXY.joystick_1_x);
           /* manage the left motor */
           Sleep_speed (LeftMotor, RemoteXY.joystick_1_y + RemoteXY.joystick_1_x);
         }
         else if (RemoteXY.speed_motor==1) {
           Wheel1 (RightMotor, RemoteXY.joystick_1_y - RemoteXY.joystick_1_x);
           /* manage the left motor */
           Wheel1 (LeftMotor, RemoteXY.joystick_1_y + RemoteXY.joystick_1_x);
         }
         else if (RemoteXY.speed_motor==2) {
           Wheel2 (RightMotor, RemoteXY.joystick_1_y - RemoteXY.joystick_1_x);
           /* manage the left motor */
           Wheel2 (LeftMotor, RemoteXY.joystick_1_y + RemoteXY.joystick_1_x);
         }
         else if (RemoteXY.speed_motor==3) {
           Wheel3 (RightMotor, RemoteXY.joystick_1_y - RemoteXY.joystick_1_x);
           /* manage the left motor */
           Wheel3 (LeftMotor, RemoteXY.joystick_1_y + RemoteXY.joystick_1_x);
         }
         else if (RemoteXY.speed_motor==4) {
           Wheel4 (RightMotor, RemoteXY.joystick_1_y - RemoteXY.joystick_1_x);
           /* manage the left motor */
           Wheel4 (LeftMotor, RemoteXY.joystick_1_y + RemoteXY.joystick_1_x);
         }
         else if (RemoteXY.speed_motor==5) {
           Wheel5 (RightMotor, RemoteXY.joystick_1_y - RemoteXY.joystick_1_x);
           /* manage the left motor */
           Wheel5 (LeftMotor, RemoteXY.joystick_1_y + RemoteXY.joystick_1_x);
         }

         digitalWrite(PIN_SAKLAR, HIGH);

         if (RemoteXY.lampu!=0) {
           digitalWrite(PIN_LAMPU, HIGH);
         }
         else {
           digitalWrite(PIN_LAMPU, LOW);
         }

         if (RemoteXY.sirene!=0) {
           digitalWrite(PIN_SIRENE, HIGH);
         }
         else
         {
           digitalWrite(PIN_SIRENE, LOW);
         }
        }
        else {
          Sleep_speed (RightMotor, RemoteXY.joystick_1_y - RemoteXY.joystick_1_x);
          /* manage the left motor */
          Sleep_speed (LeftMotor, RemoteXY.joystick_1_y + RemoteXY.joystick_1_x);
          digitalWrite(PIN_SAKLAR, LOW);
          digitalWrite(PIN_LAMPU, LOW);
          digitalWrite(PIN_SIRENE, LOW);
        }

      if (input>150 && input<200){//If the Bot is falling
        if (output>0){ //Falling towards front
        Forward(); //Rotate the wheels forward
        } else if (output<0){ //Falling towards back
        Reverse(); //Rotate the wheels backward
        }
      } else {//If Bot not falling
      Stop(); //Hold the wheels still
      }
   }
}

void  Forward ()
{
    analogWrite(PIN_MOTOR_RIGHT_UP, output);
    analogWrite(PIN_MOTOR_RIGHT_DN, LOW);
    analogWrite(PIN_MOTOR_LEFT_UP, output);
    analogWrite(PIN_MOTOR_LEFT_DN, LOW);
    lcd.setCursor(0,0);
    lcd.print("F");
    Serial.print("F");
}

void  Reverse ()
{
    analogWrite(PIN_MOTOR_RIGHT_UP, LOW);
    analogWrite(PIN_MOTOR_RIGHT_DN, output*-1);
    analogWrite(PIN_MOTOR_LEFT_UP, LOW);
    analogWrite(PIN_MOTOR_LEFT_DN, output*-1);
    lcd.setCursor(0,0);
    lcd.print("R");
    Serial.print("R");
}

void  Stop ()
{
    analogWrite(PIN_MOTOR_RIGHT_UP, LOW);
    analogWrite(PIN_MOTOR_RIGHT_DN, LOW);
    analogWrite(PIN_MOTOR_LEFT_UP, LOW);
    analogWrite(PIN_MOTOR_LEFT_DN, LOW);
    lcd.setCursor(0,0);
    lcd.print("S");
    Serial.print("S");
}