MakeBlock Bluetooth v3.0

MakeBlock_BT3.ino

/*************************************************************************
* File Name : MakeBlock_BT3.ino
* Author : David Cotterill-Drew
* Updated : 19/04/2017
* Version : V3.0
* Date : 19/04/2017
* Description : Program to be used with Orion controller/Uno
* License : Open Source
* Copyright (C) 2017 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()
{
 pinMode(2,INPUT);
 pinMode(8,INPUT);
 ledsoff();
 bluetooth.begin(115200);
}
void loop()
{
 front_check();
 range_check();
 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()
{
 //front_check();
 MotorL.run(moveSpeed);
 MotorR.run(moveSpeed);
}

void Backward()
{
 range_check();
 MotorL.run(-moveSpeed);
 MotorR.run(-moveSpeed);
}
void TurnLeft()
{
 //front_check();
 MotorL.run(-moveSpeed);
 MotorR.run(moveSpeed);
}
void TurnRight()
{
 //front_check();        //check front left & right Infra red
 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();
}

void Backward_ten()
{
  MotorL.run(-moveSpeed);
  MotorR.run(-moveSpeed);
  delay(10);
  MotorL.run(0);
  MotorR.run(0);
}


void Forward_ten()
{
  MotorL.run(moveSpeed);
  MotorR.run(moveSpeed);
  delay(10);
  MotorL.run(0);
  MotorR.run(0);
}

void TurnRight_ten()
{
  MotorL.run(moveSpeed);
  MotorR.run(-moveSpeed);
  delay(10);
  MotorL.run(0);
  MotorR.run(0);
}

void TurnLeft_ten()
{
  MotorL.run(-moveSpeed);
  MotorR.run(moveSpeed);
  delay(10);
  MotorL.run(0);
  MotorR.run(0);
}

void range_check()
{
  distance=ultraSensor.distanceCm();
  if(distance<5)
    {
      TurnRight_ten(); // Forward...
      
    }
  
}
void front_check()
{
  if(digitalRead(2)==LOW)
    {
      Backward_ten(); // turn right
      
    }
  if(digitalRead(8)==LOW)
    {
       Forward_ten(); // turn left
        
    }
  if((digitalRead(8)==LOW)&&(digitalRead(2)==LOW))
    {
      TurnLeft_ten();
      
    }
   
  
}