Glove Controlled Moving Robot

Navigation

The Project

This project was the design and construction of a robot vehicle operating based upon remote commands given through a human user utilizing a glove interface as method of control, or a Glove Controlled Moving Robot (GCMR). It is intended to function like a common remote controlled vehicle with the exception of the operating system being built into a glove rather than a remote as to enhance the user experience. In respect to these goals cost was to be minimized as much as possible while maximizing aspects of function such as speed, responsiveness, maneuverability and durability. The design was successfully competed and an operational Alpha Prototype built. Further modification and enhancement of the design may be made as deemed necessary to improve any aspect of the project.

Parts and Costs

ItemCostVendorPart Number
Chassis Kit$14.95Sparkfun.comROB-10825
Motor Driver$5.93Dealextreme.com901157149b
Redboard$19.95Sparkfun.comDEV-12757
Wireless Transceivers (x2)$7.68Amazon.comB00E594ZX0
Batteries (Robot)$1.00Dollar StoreN/A
Batteries (Control Device)$1.00Dollar StoreN/A
9v Battery clip$2.99RadioShack2700325
Arduino Nano$17.20Amazon.comB00761NDHI
Gyroscope$5.89Amazon.comB008BOPN40
Glove$10.85WalmartN/A
Wire(22-gauge solid core)$5.50RadioShackN/A
Perforated Board$3.95RadioShackN/A
LED’s$4.37Amazon.comB0060FGA8A
Metal Tape$10.99Ace Hardware47523
Total Cost$114.65

Design Description

Summary

The Glove Controlled Moving Robot operates based on data from two pieces of data, received from a gyroscope mounted on the back of the hand and two capacitors wrapped around the fore and middle fingers. The manipulation of these two sensors allows for control of the robot in the forward, backward, right and left directions at various levels of speed. The glove is controlled by a Sparkfun Redboard and the robot an Arduino Nano. The entire project is completely modular and therefore can easily be modified or repaired.

Forward and Backwards Motion

Forward and Backward control at variant speeds is achieved through the user tilting the glove to the left or the right, which motion is captured by the gyroscope mounted on the glove. As the glove is tilted to the right the robot will begin to move forward, the further the glove is tilted to the right the faster the robot will move, reaching its maximum velocity at a tilt at a little more than 90 degrees away from neutral. Similarly if the glove is tilted to the left then the robot will begin to move backwards, increasing in speed as the angle increases. It should be noted that the range for the backwards motion is smaller than that of the forward motion, due to the limitations of the human hand. If the user holds their hand flat with their palm facing the floor then the robot will be in a stopped position, moving neither forward nor backwards.

Left and Right Motion

To achieve Left and Right turning of the robot there are two capacitors mounted on the tips of the forefinger and the middle finger, with a conductive tip that has power running through it attached to the thumb. As the thumb is pressed to the forefinger the robot will begin to turn to the left, and as it is pressed to the middle finger it will move to the right. If the robot is neutral position, meaning it is not moving forward or backwards, and one of the fingers is pressed then the robot will begin to spin in place in that given direction. If the robot is moving either forwards or backwards when a finger is pressed then the robots will continue to move in that direction, forward or backward, at that speed but will begin to turn to either the right or the left, depending on which finger is pressed. If both fingers are pressed simultaneously then the robot will come to a standstill, ceasing to move in any direction regardless of the orientation of the gyroscope.

Visual Indication

For visual indication of the operation of the robot, other than seeing it move as the user executes commands, there is an array of LED lights mounted on top of both the glove and the robot that give visual confirmation of what operation the robot should be preforming. On the glove there is an array that consists of two horizontal rows of lights. In the first row there are three lights. The middle light indicates if the robot is in neutral, the right light indicates if the robot it moving forward and the left light indicates if the robot is moving backwards. If both one of the right or lights and the middle light is on then that means that the robot has reached its top speed in that direction. In the second row of the array there are two LED lights, which give indication if the robot is turning left or right. If the left light is on then the robot is turning left, and if the right lights is on it is turning right. If all of the lights are on at the same time then the both fingers are being pressed and the robot is not moving. If all the lights but the middle light in the first row is on then the glove is in sleep mode and is not moving. On the robot there is another array of lights that is used to give indication of its movement. This array is set up in an addition sign manner, with one light in the middle, one above it, one below it, one to the right of it and one to the left of it. Similar to the glove if the middle light is on then the robot is in neutral, if the light to the right of the middle light is on then it is turning right and if the left one is on then it is turning left. If the front light is on then if it is moving forward and if the back light is on then it is moving backwards. If all of the lights are on at the same time then the robot will be stationary and if the middle light and either the forward or backward lights are on then it has reached its top speed.

Sleep Mode

Built into the glove is a kill switch, so to speak, that renders all data from the glove useless and simply tells the robot to not move. This mode is switched on anytime that operational mode is on and the board is reset, either through the reset button on the redboard, the removal of power form the device or the starting up of the serial monitor when hooked up to the PC. This mode is useful when trying to do things other than control the robot with the glove on.

Programming Theory

The theory behind how the vehicle works is fairly simple in design. The glove first receives data from the sensors and processes that into two things. The first is a number between 0 and 3 that designates what state the finger capacitors are in and the second is a number between 0 and 10 that indicates what angle the gyroscope is currently at. After getting this data it breaks up into four possibilities, one for each of the different finger button states, and within these different sections of code it breaks up the 11 different possibilities that could arise from the gyroscope data, compiling a single number where the first digit tells the robot what the button state is and the next two tell it what direction and speed to run at. It is also in this part of the code that the glove decides what light to turn on. On the robot end it is first receives the single number that the glove is sending. After is has this number it breaks up the code into four different sections dictated by the first digit of the received number. After that it breaks up each section into the 11 different possibilities from the gyroscope data and tells the motors what to do in the case of each of these situations, as well as telling the lights what to do here. And then the process repeats, over and over again.

Construction

Glove

The construction of the glove is broken up into three parts: the glove, the redboard, and the shield for the redboard. The glove is a standard right handed athletic glove that can be found at any retailer store, such as Walmart. Attached to the glove at the base of the wrist is a 9v battery that is used to power the glove. Attached to the middle finger, forefinger and thumb is a conductive coating of metal tape that is connected via wires to the redboard’s shield. Attached to the back of the glove is a patch of Velcro that is used to attach the redboard. Attached to the redboard itself is a matching patch of Velcro that is used to connect the redboard to the glove. Attached to the top of the redboard is the homemade shield that attaches all of the working components to the redboard. The shield is made out of simple perforated board with all of the different pieces soldered in and a set of breakout pins for it to attach to the redboard. The nRF24 and mpu 6050 breakout boards have spacers mounted underneath them to help prevent damages and improve durability.

Robot

The robot is made up of two different parts, the chassis and the Arduino shield. For the chassis it was simply built to instructions and operates just like it should. Attached to the chassis is the shield. It is attached via holes that were drilled into the top piece that were then used to attach two strips of plywood that act as a spacer for the shield. Attached to the top of the plywood is the homemade Arduino Nano shield. It is also made out of simple perforated board with all of the components soldered on. The nRF24 and motor driver breakout boards again have spacers underneath them to improve durability and the battery pack has a separate plug in as to simplify turning the robot on and off.

Code

Glove

//Radio Libaries
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"

//Gyroscope Libaries
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif
MPU6050 mpu;

//General Libaries
#include <EEPROM.h>

//Radio stuff
RF24 radio(9,10);
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };

//Groscope Stuff
bool dmpReady = false;  
uint8_t mpuIntStatus;  
uint8_t devStatus;
uint16_t packetSize; 
uint16_t fifoCount;     
uint8_t fifoBuffer[64]; 

Quaternion q;           
float ypr[3]; 
VectorFloat gravity;
int gyrodata;

volatile bool mpuInterrupt = false;

//Button Stuff
const int buttonLeft = A0;
const int buttonRight = A1;
int buttonStates[2];
//int buttonstate[2]; //buttonstate[0] is left, buttonstate[1] is right

//Light Stuff
const int LED_TURN_RIGHT = 4; 
const int LED_TURN_LEFT = 5;
const int LED_MIDDLE = 6;
const int LED_TILT_RIGHT = 3;
const int LED_TILT_LEFT = 7;

//Motor Stuff
int motorData;
int gyroreverse;

//General Stuff
int sensorData[2];
float constrainedgyro; 
byte buttonPushCounter = EEPROM.read(4);

void setup(void)
{
  Serial.begin(38400);

  EEPROM.write(4, buttonPushCounter+1);

    radiosetup();
    gyroscopesetup();
    buttonsetup();
    lightsetup();
    Serial.println(buttonPushCounter);
}

void loop(void)
{
  buttonPushCounter =EEPROM.read(4);
  if(buttonPushCounter % 2 == 0)
  {
    digitalWrite(LED_TURN_RIGHT, HIGH);
    digitalWrite(LED_TURN_LEFT, HIGH);
    digitalWrite(LED_MIDDLE, LOW);
    digitalWrite(LED_TILT_RIGHT, HIGH);
    digitalWrite(LED_TILT_LEFT, HIGH);
    motorData = 0;
    Serial.println("kill code");
  }

  else
  {

  getsensorData();
 switch (sensorData[0])
 {
   case 0:
   //brake
    digitalWrite(LED_TURN_RIGHT, HIGH);
    digitalWrite(LED_TURN_LEFT, HIGH);
    digitalWrite(LED_MIDDLE, HIGH);
    digitalWrite(LED_TILT_RIGHT, HIGH);
    digitalWrite(LED_TILT_LEFT, HIGH);
    motorData = 0;

   break;
   case 1:
   //no buttons pushed, forward backward
   usegyrodata();
   motorData = gyrodata + 100;

   break;
   case 2:
   //left turn button
   usegyrodata();
   digitalWrite(LED_TURN_LEFT, HIGH);
   motorData = gyrodata + 200;

   break;
   case 3:
   //right turn button
   usegyrodata();
   digitalWrite(LED_TURN_RIGHT, HIGH);
   motorData = gyrodata + 300;

   break;
 }
  }
 radio.write( &motorData , sizeof(motorData) );
}

void radiosetup()
{
  radio.begin();
  //radio.setRetries(10,10);
  //radio.setPayloadSize(8);
  SPI.setClockDivider(SPI_CLOCK_DIV2);
  radio.setDataRate(RF24_2MBPS);
  radio.openWritingPipe(pipes[0]);
  radio.stopListening();
  //radio.printDetails();
}

void getsensorData()
{
  getbuttonstate();
  getgyrodata();
}

void getbuttonstate()
{
  buttonStates[0] = analogRead(buttonLeft);
  buttonStates[1] = analogRead(buttonRight);

  if(buttonStates[0] < 10 && buttonStates[1] < 10)
  {
    sensorData[0] = 1;
  }
  else if (buttonStates[0] < 10 && buttonStates[1] > 10)
  {
    sensorData[0] = 2;
  }
  else if (buttonStates[0] > 10 && buttonStates[1] < 10)
  {
    sensorData[0] = 3;
  }
  else 
  {
    sensorData[0] = 0;
  }
}

void gyroscopesetup()
{
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; 
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    mpu.initialize();
    devStatus = mpu.dmpInitialize();

    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); 

    mpu.setDMPEnabled(true);
    //attachInterrupt(0, dmpDataReady, RISING);
    packetSize = mpu.dmpGetFIFOPacketSize();
}

void buttonsetup()
{

}

void lightsetup()
{
  pinMode(LED_TURN_RIGHT, OUTPUT);
  pinMode(LED_TURN_LEFT, OUTPUT);
  pinMode(LED_MIDDLE, OUTPUT);
  pinMode(LED_TILT_RIGHT, OUTPUT);
  pinMode(LED_TILT_LEFT, OUTPUT);
}

void getgyrodata()
{
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    fifoCount = mpu.getFIFOCount();

    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        mpu.resetFIFO();
        Serial.println("FIFO OVERLOAD");

    } else if (mpuIntStatus & 0x02) {

        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        mpu.getFIFOBytes(fifoBuffer, packetSize);

        fifoCount -= packetSize;

            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            constrainedgyro = ypr[2] * 180/M_PI;
    }
  }

void usegyrodata()
{
  sensorData[1] = constrain(constrainedgyro, -80, 80);
  gyrodata = map(sensorData[1], 70, -60, 0, 10);

  //Serial.print(sensorData[1]);
 // Serial.print(" ");
  Serial.println(gyrodata);

  if(gyrodata == 5 || gyrodata == 4)
  {
    digitalWrite(LED_TURN_RIGHT, LOW);
    digitalWrite(LED_TURN_LEFT, LOW);
    digitalWrite(LED_MIDDLE, HIGH);
    digitalWrite(LED_TILT_RIGHT, LOW);
    digitalWrite(LED_TILT_LEFT, LOW);


  }
  else if(gyrodata < 4)
  {
    digitalWrite(LED_TURN_RIGHT, LOW);
    digitalWrite(LED_TURN_LEFT, LOW);
    digitalWrite(LED_MIDDLE, LOW);
    digitalWrite(LED_TILT_RIGHT, LOW);
    digitalWrite(LED_TILT_LEFT, HIGH);

    if (gyrodata  == 0)
    {
      digitalWrite(LED_MIDDLE, HIGH);
    }
  }
  else if( gyrodata > 5)
  {
    digitalWrite(LED_TURN_RIGHT, LOW);
    digitalWrite(LED_TURN_LEFT, LOW);
    digitalWrite(LED_MIDDLE, LOW);
    digitalWrite(LED_TILT_RIGHT, HIGH);
    digitalWrite(LED_TILT_LEFT, LOW);

    if (gyrodata  == 10)
    {
      digitalWrite(LED_MIDDLE, HIGH);
      //digitalWrite(LED_TILT_RIGHT, HIGH);
    }
  }
}

Robot

//Radio Libaries
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"

//Radio Stuff
RF24 radio(8,9);
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
int sensorData;

//Motor Driver Stuff
const int AIA = 10;
const int AIB = 3;
const int BIA = 5;
const int BIB = 6;

//Lights Stuff
const int LED_RIGHT = A1;
const int LED_LEFT = A5;
const int LED_MIDDLE = A2;
const int LED_FORWARD = A3;
const int LED_BACK = A4;

void setup(void)
{
  Serial.begin(38400);
  radiosetup();
  motorsetup();
  lightsetup();
}

void loop(void)
{

  //radioread();
  if ( radio.available() )
    {

      bool done = false;
      while (!done)
      {
        done = radio.read( &sensorData, sizeof(sensorData) );
      }
    }

   Serial.println(sensorData);

   if(sensorData == 0)
   {
     mstop();
     digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, HIGH);
     digitalWrite(LED_MIDDLE, HIGH);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, HIGH);
   }

   else if (sensorData > 99 && sensorData < 199)
   {
     //going forward or back
     switch (sensorData)
     {
       case 100:
       //back
       reverse(255);
       digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, HIGH);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH);

       break;
       case 101:
       // back
       reverse(200);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH);
       break;
       case 102:
       //back
       reverse(175);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH);
       break;
       case 103:
       //back
       reverse(150);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH);
       break;
       case 104:
       //stop
       mstop();
     digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, HIGH);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, LOW);

       break;
       case 105:
       //stop
        mstop();
     digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, HIGH);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, LOW);

       break;
       case 106:
       //forward
      forward(150);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW); 
       break;
       case 107:
       //forward
       forward(175);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW); 
       break;
       case 108:
       //forward
       forward(200);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW); 
       break;
       case 109:
       //forward
       forward(225);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW); 
       break;
       case 110:
       //forward
       forward(250);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, HIGH);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW); 
       break;

     }
   }

   else if (sensorData > 199 && sensorData < 299)
   {
     switch (sensorData)
     {
       case 200:
       leftwheelback(255);
       rightwheelback(250);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, HIGH);
     digitalWrite(LED_MIDDLE, HIGH);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH); 
       break;
       case 201:
       leftwheelback(200);
       rightwheelback(225);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, HIGH);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH); 

       break;
       case 202:
       leftwheelback(175);
       rightwheelback(200);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, HIGH);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH); 
       break;
       case 203:
       leftwheelback(150);
       rightwheelback(175);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, HIGH);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH); 
       break;
       case 204:
        left(155);
        digitalWrite(LED_RIGHT, LOW);
        digitalWrite(LED_LEFT, HIGH);
        digitalWrite(LED_MIDDLE, HIGH);
        digitalWrite(LED_FORWARD, LOW);
        digitalWrite(LED_BACK, LOW);

       break;
       case 205:
        left(155);
        digitalWrite(LED_RIGHT, LOW);
        digitalWrite(LED_LEFT, HIGH);
        digitalWrite(LED_MIDDLE, HIGH);
        digitalWrite(LED_FORWARD, LOW);
        digitalWrite(LED_BACK, LOW);

       break;
       case 206:
       leftwheel(150);
       rightwheel(175);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, HIGH);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW); 
       break;
       case 207:
       leftwheel(175);
       rightwheel(200);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, HIGH);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW);
       break;
       case 208:
       leftwheel(200);
       rightwheel(225);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, HIGH);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW);
       break;
       case 209:
       leftwheel(225);
       rightwheel(255);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, HIGH);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW);
       break;
       case 210:
       leftwheel(200);
       rightwheel(255);
        digitalWrite(LED_RIGHT, LOW);
     digitalWrite(LED_LEFT, HIGH);
     digitalWrite(LED_MIDDLE, HIGH);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW);
       break;

     }
   }

   else
   {
     switch (sensorData)
     {
       case 300:
       leftwheelback(255);
       rightwheelback(225);
        digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, HIGH);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH); 
       break;
       case 301:
       leftwheelback(225);
       rightwheelback(200);
        digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH);
       break;
       case 302:
       leftwheelback(200);
       rightwheelback(175);
        digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH);
       break;
       case 303:
       leftwheelback(175);
       rightwheelback(150);
        digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, HIGH);
       break;
       case 304:
        right(155);
     digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, HIGH);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, LOW);

       break;
       case 305:
        right(155);
     digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, HIGH);
     digitalWrite(LED_FORWARD, LOW);
     digitalWrite(LED_BACK, LOW);

       break;
       case 306:
       leftwheel(175);
       rightwheel(150);
        digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW);
       break;
       case 307:
       leftwheel(200);
       rightwheel(175);
        digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW);
       break;
       case 308:
       leftwheel(225);
       rightwheel(200);
        digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW);
       break;
       case 309:
       leftwheel(255);
       rightwheel(200);
        digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW);
       break;
       case 310:
       leftwheel(255);
       rightwheel(225);
        digitalWrite(LED_RIGHT, HIGH);
     digitalWrite(LED_LEFT, LOW);
     digitalWrite(LED_MIDDLE, LOW);
     digitalWrite(LED_FORWARD, HIGH);
     digitalWrite(LED_BACK, LOW);
       break;

     }
   }
}

void radiosetup()
{
  radio.begin();
  //radio.setRetries(10,10);
  //radio.setPayloadSize(8);
  SPI.setClockDivider(SPI_CLOCK_DIV2);
  radio.setDataRate(RF24_2MBPS);
  radio.openReadingPipe(1,pipes[0]);
  radio.startListening();
  //radio.printDetails();
}

void radioread()
{
  if ( radio.available() )
    {

      bool done = false;
      while (!done)
      {
        done = radio.read( &sensorData, sizeof(sensorData) );
      }
    }
}

void motorsetup()
{
  pinMode(AIA, OUTPUT);
  pinMode(AIB, OUTPUT);
  pinMode(BIA, OUTPUT);
  pinMode(BIB, OUTPUT);

}

void lightsetup()
{
  pinMode(LED_RIGHT, OUTPUT);
  pinMode(LED_LEFT, OUTPUT);
  pinMode(LED_MIDDLE, OUTPUT);
  pinMode(LED_FORWARD, OUTPUT);
  pinMode(LED_BACK, OUTPUT);
}



void forward(int speed)
{
  analogWrite(AIA, speed);
  analogWrite(AIB, 0);
  analogWrite(BIA, speed);
  analogWrite(BIB, 0);
}

void reverse(int speed)
{
  analogWrite(AIA, 0);
  analogWrite(AIB, speed);
  analogWrite(BIA, 0);
  analogWrite(BIB, speed);
}

void left(int speed)
{
  analogWrite(AIA, speed);
  analogWrite(AIB, 0);
  analogWrite(BIA, 0);
  analogWrite(BIB, speed);
}

void right(int speed)
{
  analogWrite(AIA, 0);
  analogWrite(AIB, speed);
  analogWrite(BIA, speed);
  analogWrite(BIB, 0);
}

void leftwheel(int speed)
{
 analogWrite(AIA, speed);
  analogWrite(AIB, 0);
}

void rightwheel(int speed)
{
  analogWrite(BIA, speed);
  analogWrite(BIB, 0);
}

void leftwheelback(int speed)
{
 analogWrite(AIA, 0);
  analogWrite(AIB, speed);
}

void rightwheelback(int speed)
{
  analogWrite(BIA, 0);
  analogWrite(BIB, speed);
}

void mstop()
{
  analogWrite(AIA, 0);
  analogWrite(AIB, 0);
  analogWrite(BIA, 0);
  analogWrite(BIB, 0);
}

Share