MakeBlock Bluetooth Arduino robot forklift

MakeBlock_BT2.ino

/*************************************************************************
* File Name : MakeBlock_BT2.ino
* Author : David Cotterill-Drew
* Updated : 18/04/2017
* Version : V2.0
* Date : 18/04/2017
* Description : Program to be used with Mbot
BT.py to control robot via bluetooth.
* License : Open Source
* Copyright (C) 2016 RoboTonics.
* https://r0b0t.wordpress.com/
**************************************************************************/
#include "MeOrion.h"
#include <Arduino.h>
#include <SoftwareSerial.h>
#include <Wire.h>

MeDCMotor MotorL(M1);
MeDCMotor MotorR(M2);
MeDCMotor Holder(PORT_1);
MeDCMotor Hand(PORT_2);
MeRGBLed led(PORT_3);
MeUltrasonicSensor ultraSensor(PORT_7);
MeBluetooth bluetooth(PORT_5);

int distance=0;
int HolderSpeed = 250;
int HandSpeed = 250;
int moveSpeed = 190;
boolean leftflag,rightflag;
int minSpeed = 55;
int factor = 23;
int range=0;

void setup()
{

 ledsoff();
 bluetooth.begin(115200);
}
void loop()
{
 int inByte = bluetooth.read();
 if(inByte=='f')
 {
 TurnRight();
 }
 if(inByte=='s')
 {
 Stop();
 }
 if(inByte=='l')
 {
 Backward();
 }
 if(inByte=='b')
 {
 TurnLeft();
 }
 if(inByte=='r')
 {
 Forward();
 }
 if(inByte=='a')
 {
 Holder_down();
 }
 if(inByte=='c')
 {
 Holder_up();
 }
 if(inByte=='e')
 {
 Hand_close();
 }
 if(inByte=='d')
 {
 Hand_open();
 }
 if(inByte=='1')
 {
 ChangeSpeed(factor*1+minSpeed);
 }
 if(inByte=='2')
 {
 ChangeSpeed(factor*2+minSpeed);
 }
 if(inByte=='3')
 {
 ChangeSpeed(factor*3+minSpeed);
 }
 if(inByte=='4')
 {
 ChangeSpeed(factor*4+minSpeed);
 }
 if(inByte=='5')
 {
 ChangeSpeed(factor*5+minSpeed);
 }
 if(inByte=='6')
 {
 ChangeSpeed(factor*6+minSpeed);
 }
 if(inByte=='7')
 {
 ChangeSpeed(factor*7+minSpeed);
 }
 if(inByte=='8')
 {
 ChangeSpeed(factor*8+minSpeed);
 }
 if(inByte=='9')
 {
 ChangeSpeed(factor*9+minSpeed);
 }
 if(inByte=='g')
 {
 ledsoff();
 }
 if(inByte=='h')
 {
 ledson();
 }
 if(inByte=='x')
 {
 ledsblue();
 }
 if(inByte=='y')
 {
 ledsred();
 }
 if(inByte=='z')
 {
 ledsgreen();
 }
}
void Forward()
{
 MotorL.run(moveSpeed);
 MotorR.run(moveSpeed);
}
void Backward()
{
 MotorL.run(-moveSpeed);
 MotorR.run(-moveSpeed);
}
void TurnLeft()
{
 MotorL.run(-moveSpeed);
 MotorR.run(moveSpeed);
}
void TurnRight()
{
 MotorL.run(moveSpeed);
 MotorR.run(-moveSpeed);
}
void Stop()
{
 MotorL.run(0);
 MotorR.run(0);
 Holder.run(0);
 Hand.run(0);
}
void ChangeSpeed(int spd)
{
 moveSpeed = spd;
}
void Holder_up()
{
 Holder.run(HolderSpeed);
}
void Holder_down()
{
 Holder.run(-HolderSpeed);
}
void Hand_close()
{
 Hand.run(HandSpeed);
}
void Hand_open()
{
 Hand.run(-HandSpeed);
}
 
void ledson()
{
 led.setColorAt(0, 255, 255, 255);
 led.setColorAt(1, 255, 255, 255);
 led.setColorAt(2, 255, 255, 255);
 led.setColorAt(3, 255, 255, 255);
 led.show();
}
void ledsoff()
{
 led.setColorAt(0, 0, 0, 0);
 led.setColorAt(1, 0, 0, 0);
 led.setColorAt(2, 0, 0, 0);
 led.setColorAt(3, 0, 0, 0);
 led.show();
}
void ledsblue()
{
 led.setColorAt(0, 0, 0, 255);
 led.setColorAt(1, 0, 0, 255);
 led.setColorAt(2, 0, 0, 255);
 led.setColorAt(3, 0, 0, 255);
 led.show();
}
void ledsred()
{
 led.setColorAt(0, 255, 0, 0);
 led.setColorAt(1, 255, 0, 0);
 led.setColorAt(2, 255, 0, 0);
 led.setColorAt(3, 255, 0, 0);
 led.show();
}
void ledsgreen()
{
 led.setColorAt(0, 0, 255, 0);
 led.setColorAt(1, 0, 255, 0);
 led.setColorAt(2, 0, 255, 0);
 led.setColorAt(3, 0, 255, 0);
 led.show();
}