Remote Control Gripper Bot
A few months back my son asked me to make him a remote controlled bot. He gave me a list of what he wanted and we narrowed it down to a manageable list :) This is what I ended up building for him...
Meet Clamps, a mobile remote gripper robot. He is controlled via a Python Tkinter application that sends commands to the micro controller inside the bot through a serial Bluetooth connection between the PC and the controller.
Clamps is controlled using the HC05 bluetooth module, an Arduino UNO clone, V5 sensor shield, a L9110s motor driver and is powered by 5 "AA" rechargeable NiMH to run the servos and micro controller and one 7.2v li-ion for the motors. For the chassis, I use the Actobotics Half-pint for a few reasons that will become clear.
From the beginning, I wanted a chassis that could support a some sort of 2 degree of freedom gripper (2DOF). A chassis that also would house all of the electronics and have enough room to grow with. The Half-pint fits all of these requirements nicely. It's a 4wd platform, has a large enclosed area for the electronics and can support a 2DOF gripper assembly.
For this project you could make your own chassis or choose another, the choice is yours.
The Software Needed
Things you will need to download:
- The Arduino IDE
- Python 2.7 (a full install with tkinter)
- The pySerial library
- The L9110S motor controller library attached .zip (See the files in this Step)
- The Robot code below or the attached .zip (See the files in this Step)
- The Python remote control application (See the files in this Step)
-
SoftwareSerial library (Used to configure the bluetooth adaptor)
Hardware Bill of Materials
Parts you will need are as follows:
- Runt Rover Half-pint chassis or a suitable 4wd enclosed replacement
- Arduino Uno or similar board with USB cable
- V5 sensor shield
- L9110S motor driver (Get a couple as they are cheap)
- MG995 servo or another suitable replacement x2
- HC-05 or HC-06 bluetooth adaptor x1
- Standard servo plate B x1
- 90 degree bracket x1
- Single bracket x2
- Lightweight servo hub (525125 for Futaba) x1
- Standard Gripper Kit A x1
- Female to Female jumper wires
- 5 rechargeable AA batteries (NiMH) and charger
- Double sided sticky tape and small zip-ties
- A 'AA' 5 slot battery pack
- 6v NiMH or 7.4v Li-ion battery pack and charger
- 1.250" 6-32 screws x2
- .750" 8-32 nylon spacers x2
- .500" 6-32 screws with nuts x8
- .3125" 6-32 screws x8
You can source many of the parts from eBay or other suppliers directly like Sparkfun, RobotShop and ServoCity.
After you have purchased, collected and downloaded everything thing you're ready to start with the build.
Assembly - the Chassis
First assemble the chassis following the instructions included with the chassis or video. After finishing you should have something like the image. When you place the plate that holds the controller on the bot, make sure you place it in the middle holes. This will leave space in the front and the rear of the bot.
NOTE: Before going any further, please remove all 4 wheels, so as not to damage the motors whilst putting the other bot components on.
Assembling the Gripper
Assemble gripper following the instructions provided in this video. After finishing you should have something like the image.
Mounting the Gripper - Step 1
Take the 90 degree bracket, lightweight servo hub and four (4) of the .3125" screws for this step:
- Take the servo hub and place it on one side of the bracket and secure them together with the .2125" screws like pictured and set it aside
- Place the nuts on the two shorter screws and tighten them down
- Now tale the two Single brackets, the 1.250" screws and two .500" screws with nuts, 2 nylon spacers and the Servo plate B. Take one single bracket and place in in the inside of the Half-pint bot and run the 2 1.259" screws on on the left holes (from the inside) and the 2 .500" screws on the right side. All of the screws heads will be on the inside of the bot as pictured. now place the second single bracket on the screws on the outside of the bot. Next take the nylon spacers and place them on the longer (1.250") sticking out and then screw the servo plate onto the long screws
- Tighten down the screws going to the servo plate with a hex key of the proper size or screw driver if you used normal screws.
Use the pictures for reference as needed.
Mounting the Gripper - Step 2
- Next take the remaining servo you have and place it in the servo bracket with the spine to the top. Secure it in with 4 of the .3125" screws.
- Next take the 90 degree bracket with the servo hub on it and mount the gripper on to it with the 4 of the .500" screws using 2 of the 8-32 nuts as spacers between the bracket and the gripper as pictured. Secure with another nut on top of the gripper (on under the bracket if you turn the screws the other way)
- Now take the gripper assembly and place it onto the servo and use a servo screw (from your servo packet) to hold it in-place, firmly securing it.
After you are finished, run the two servo wires though the flat bracket hole to the inside of the bot.
See the pictures for more detail and reference.
Mounting the Electronics
- Take the UNO and V5 Sensor shield and snap them together.
- Place the joined UNO/Sensor board into the clip holder inside the center of the bot.
- Take Double sided tape and place the motor driver on the left inside wall and the bluetooth adaptor on the right wall.
Wiring - the Motors
- A-1A pin on the motor driver to pin 5 on the sensor shield
- A-1B pin on the motor driver to pin 6 on the sensor shield
- B-1A pin on the motor driver to pin 3 on the sensor shield
- B-1B pin on the motor driver to pin 11 on the sensor shield
Now with the robots front facing AWAY from you, take the wires on the LEFT side and -
- Connect the two BLACK wires on the left to the first output screw terminal for Motor A
- Connect the two RED wires on the left to the second output screw terminal for Motor A
Take the wires on the RIGHT side and -
- Connect the two BLACK wires on the left to the first output screw terminal for Motor B
- Connect the two RED wires on the left to the second output screw terminal for Motor B
The Servos:
- Now connect the Y servo wire to pin line 2 on the sensor shield. The white or orange wire is always the signal wire.
- Now connect the X (Gripper) servo wire to pin line 7 on the sensor shield. The white or orange wire is always the signal wire.
Wiring the Bluetooth Module
If you need to configure the Bluetooth adaptor wire it this way first:
- Bluetooth adaptor Tx -> sensor shield pin 7
- Bluetooth adaptor Rx -> sensor shield pin 8
If you do not need to configure your adaptor, then wire it this way:
- Bluetooth adaptor Tx -> sensor shield pin 0
- Bluetooth adaptor Rx -> sensor shield pin 1
NOTE: After you are done with the adaptor configuration, you will place the gripper servo on pin 7.
Adding Power
Connect the power leads from the 5 'aa' (or other 6v pack) battery pack to the sensor shield's power screw terminal:
- Red to Vcc
- Black to GND
- Next make sure the jumper on the V5 sensor shield is in place on the jumper pins.
- Connect the GND pin on the L9110s driver to a GND pin on the sensor shield.
- Connect the Positive wire from the 7.2v battery or other battery you have chosen for the motors to the Vcc pin on the L9110S motor driver.
- Connect the Negative (GND) wire from the motor battery a GND pin on the motor shield.
Configuring the HC05 Bluetooth Module
- The HC-05 and HC-06 bluetooth adaptors are set to a default rate of 9600 when you first get them new. For our bot, we want it running at a fast baud rate and will use the code below to set it to 57600. This code comes from the Johnny Five project and to run it you will need the SoftwareSerial library that can be found here.
To run this setup make sure you have the Bluetooth adaptor Tx pin connected to the sensor shields pin 7 and the Bluetooth adaptor Rx pin to the sensor shields pin 8. Once this is done, run the following code to reset the baud rate and set the adaptors info.
NOTE: If you need information on compiling Arduino code, see Step 12: Loading the code.
// Taken from the Johnny Five Bluetooth setup page:
// <a href="https://github.com/rwaldron/johnny-five/wiki/Getting-Started-with-Johnny-Five-and-JY-MCU-Bluetooth-Serial-Port-Module" rel="nofollow"> <a rel="nofollow"> <a rel="nofollow"> <a rel="nofollow"> <a rel="nofollow"> <a rel="nofollow"> <a rel="nofollow"> https://github.com/rwaldron/johnny-five/wiki/Gett...</a>>>>>>>
#define ROBOT_NAME "CLAMPS" // Change if you want to a different name
// If you haven't configured your device before use this
#define BLUETOOTH_SPEED 9600
// If you are modifying your existing configuration, use this:
//#define BLUETOOTH_SPEED 57600
#include <SoftwareSerial.h>
// Swap RX/TX connections on bluetooth chip
// Pin 7 --> Bluetooth TX
// Pin 8 --> Bluetooth RX
SoftwareSerial mySerial(7, 8); // RX, TX
void setup ( )
{
Serial.begin (9600);
while (!Serial);
Serial.println ("Starting config");
mySerial.begin (BLUETOOTH_SPEED);
delay (1000);
// Should respond with OK
mySerial.print ("AT");
waitForResponse ( );
// Should respond with its version
mySerial.print ("AT+VERSION");
waitForResponse ( );
// Set pin to 0000
mySerial.print ("AT+PIN0000");
waitForResponse ( );
// Set the name to ROBOT_NAME
mySerial.print ("AT+NAME");
mySerial.print (ROBOT_NAME);
waitForResponse ( );
// Set baudrate to 57600
mySerial.print ("AT+BAUD7");
waitForResponse ( );
Serial.println ("Done!");
}
void waitForResponse ( ) {
delay (1000);
while (mySerial.available ( )) {
Serial.write(mySerial.read ( ));
}
Serial.write ("\n");
}
void loop () { }
Once the code has been loaded and run on the Arduino, the Bluetooth module should be ready to use on the bot.
Take the Bluetooth adaptor Tx pin connected to the sensor shields pin 7 and move it to pin 0 on the shield, Then move the Bluetooth adaptor Rx pin from the shields pin 8 to pin 1.
!!NOTE!!: After you are done with the adaptor configuration, you will place the gripper servo on pin 7.
This will give you:
- Bluetooth adaptor Tx -> sensor shield pin 0
- Bluetooth adaptor Rx -> sensor shield pin 1
- Gripper servo on pin 7's line
Now you will need to go into the bluetooth device settings on your PC from the OS's "Control" or "Systems" Panel or use Google to help.
Possible OS Bluetooth setup information:
You will need to note the serial port device name or number that is assigned to it.
NOTE: If your Bluetooth adaptor has already been configured or you want to just run it at 9600 baud, you can skip this step. If you DO want to just run the code at 9600 you will have to change the baud rate in both the Python and Arduino code from 57600 to 9600. Just a single line change in both programs.
Loading the Code
With the wiring checked and rechecked, It's time to load the code. Follow the instructions on the Arduino site on how to install the Arduino IDE.
After the IDE is installed, you can next install the L9110 Motor Driver library. Do this by downloading the zip file included in STEP 1 named L9110Driver.zip and extracting it to your Arduino libraries folder or by following these instructions on how to add a library.
Now that the IDE and Motor driver library have been installed, load the robot code into the Arduino IDE from the zip file found in STEP 1 named blue_t_slave.zip. These files are also included in this step as well.
Connect your PC and Arduino up with the USB cable. Now choose the board from the Tools->Board menu in the IDE, Uno for this project (choose the board you have if different from Uno) Now from Tools->Port menu, pick your com port. After this is done, click the upload button. If all went well, the code was loaded, if not see here for help on the IDE and related issues.
Running the Python Control Application
To run the Python remote control application, download the zip file from this step and extract it to the location you want to run it from. Next, open a command shell (i.e. terminal, cmd.exe, etc...) and navigate to the directory you extracted the file to. Now type: python rover.py from the command line and a window looking like the one in the image should pop up. If not, look for any python errors and correct (i.e. missing libraries etc...) The Python site may help some if needed.
Once the App is running, you should be ready go.
Now, power on the bot. Then type the comm port of your adaptor and click the "Connect" button. After a few seconds you should see the other controls enabled and the connect button disabled. This means you are connected to the robot. If you can't connect to the bluetooth adaptor, you will have to use the magic of Google to help you out!
The control application is simple to use and allows for mouse or keyboard controls.
The keyboard controls are:
- Arrows are forward, reverse, turn left 90 degrees, turn right 90 degrees
- a - turn left 45 degrees
- s - turn right 45 degrees
- h - halt (stop)
- y - set Gripper Y axis
- u - set gripper open/close
- c - gripper home
Forward and Reverse commands are constant i.e. they keep the bot moving after execution until a new direction is sent or a Halt command.
The turns 90 and 45 degrees are temporary i.e. After some delay, they stop the bot from moving.
The gripper sliders do not automatically set the gripper on the bot. You have to hit the corresponding "Set" button or key to do the actual execution of the set.
The Slider values range from 0-180.
- Gripper Y Axis: 0 is All the way up and 180 is all the way down.
- Gripper Open/Close: 0 is all the way closed and 180 and all the way open.
Use the "Disconnect" button to stop using the program. This will send commands to stop the bot and home the gripper.
Downloads
Clamps in Action!
Code Listing for the Python Application
#!/usr/bin/env python
#
# For Linux, BSD or Mac OSX you can chmod +x on this script to make executable
#
###########
# Rover control app
#
# Written by Scott Beasley - 2015
# Free to use or modify. Enjoy.
#
###########
import sys
import serial
import time
from Tkinter import *
import tkFont
import tkMessageBox
# Create the window for the application
class App (Frame):
# Make the window
def createWidgets (self):
self.connected = False
self.message = StringVar ( )
# Make a little font for the gripper set buttons.
helv6 = tkFont.Font (family = 'Helvetica',
size = 6, weight = 'normal')
self.frame = Frame (self.master)
self.frame.pack ( )
self.f1 = Frame (self.frame)
self.l1 = Label (self.f1, text = "Comm Port: ")
self.l1.pack (side = LEFT)
self.comm_entry = Entry (self.f1, bd = 5, name = "comm_entry")
self.comm_entry.pack (side = LEFT)
self.connectButton = Button (self.f1, text = "Connect",
command = self.SerialConnect,
name = "b_connect")
self.connectButton.pack (side = LEFT)
self.disconnectButton = Button (self.f1, text = "Disconnect",
command = self.SerialDisconnect,
name = "b_disconnect")
self.disconnectButton.pack (side = RIGHT)
self.f1.grid (row = 0, column = 0)
self.f2 = LabelFrame (self.frame, bd = 3, relief = "groove",
text="Ground Control")
self.g_vert_fm = Frame (self.f2)
self.grip_vert = Scale (self.g_vert_fm, from_ = 0, to = 180)
self.grip_vert.grid (row = 0, column = 0, rowspan = 4, sticky = W)
self.grip_vert_set = Button (self.g_vert_fm, text = "Set",
command = self.GripperY,
name = "b_grip_vert_set",
width = 1, height = 2,
font = helv6)
self.grip_vert_set.grid (row = 5, column = 0, sticky = W)
self.master.bind ("", self.GripperY)
self.g_vert_fm.grid (row = 0, column = 0, rowspan = 4, sticky = W)
self.leftforwardButton = Button (self.f2, text = "\\",
command = self.TurnLeft45,
name = "b_left_forward")
self.leftforwardButton.grid (row = 0, column = 1)
self.master.bind ("<a rel="nofollow">", self.TurnLeft45)
self.leftButton = Button (self.f2, text = "<",
command = self.TurnLeft, name = "b_left")
self.leftButton.grid (row = 1, column = 1)
self.master.bind ("</a><a rel="nofollow">", self.TurnLeft)
self.rightforwardButton = Button (self.f2, text = "/",
command = self.TurnRight45,
name = "b_right_forward")
self.rightforwardButton.grid (row = 0, column = 3)
self.master.bind ("</a><a rel="nofollow">", self.TurnRight45)
self.haltButton = Button (self.f2, text = "Halt!",
command = self.Halt, name = "b_halt")
self.haltButton.grid (row = 1, column = 2)
self.master.bind ("</a><a rel="nofollow">", self.Halt)
self.rightButton = Button (self.f2, text=">",
command = self.TurnRight, name = "b_right")
self.rightButton.grid(row = 1, column = 3)
self.master.bind ("</a><a rel="nofollow">", self.TurnRight)
self.upButton = Button (self.f2, text="^",
command=self.Forward, name = "b_forward")
self.upButton.grid (row = 0, column = 2)
self.master.bind ("</a><a rel="nofollow">", self.Forward)
self.leftdownButton = Button (self.f2, text = "/",
command = self.TurnRight45,
name = "b_left_down")
self.leftdownButton.grid (row = 2, column = 1)
self.downButton = Button (self.f2, text="V",
command=self.Reverse, name = "b_reverse")
self.downButton.grid (row=2, column = 2)
self.master.bind ("</a><a rel="nofollow">", self.Reverse)
self.f2.grid (row = 1, column = 0, pady = 25)
self.rightdownButton = Button (self.f2, text = "\\",
command = self.TurnLeft45,
name = "b_right_down")
self.rightdownButton.grid (row = 2, column = 3)
self.g_horz_fm = Frame (self.f2)
self.grip_horz = Scale (self.g_horz_fm, from_ = 0, to = 180,
orient = HORIZONTAL)
self.grip_horz.grid (row = 0, column = 0, columnspan = 7, sticky = E)
self.grip_horz_set = Button (self.g_horz_fm, text = "Set",
command = self.GripperX,
name = "b_grip_horz_set",
width = 1, height = 2,
font = helv6)
self.grip_horz_set.grid (row = 0, column = 8)
self.master.bind ("<u>", self.GripperX)
self.g_horz_fm.grid (row = 4, column = 0, columnspan = 7, sticky = E)
self.master.bind ("</u></a><a rel="nofollow"><u>", self.GripperHome)
self.f3 = Frame (self.frame)
self.l2 = Label (self.f3, text = "Last action: ")
self.l2.pack (side = LEFT)
self.l3 = Label (self.f3, text=" ", textvariable = self.message)
self.l3.pack (side = RIGHT)
self.f3.grid (row = 3, column = 0, pady = 8)
# Set the state of the bot control buttons. Enable when connected,
# Disabled otherwise.
def CtrlButtonsState (self, bstate):
self.leftforwardButton.config (state = bstate)
self.leftButton.config (state = bstate)
self.rightforwardButton.config (state = bstate)
self.rightButton.config (state = bstate)
self.upButton.config (state = bstate)
self.leftdownButton.config (state = bstate)
self.downButton.config (state = bstate)
self.rightdownButton.config (state = bstate)
self.haltButton.config (state = bstate)
self.disconnectButton.config (state = bstate)
self.grip_horz.config (state = bstate)
self.grip_vert.config (state = bstate)
self.grip_horz_set.config (state = bstate)
self.grip_vert_set.config (state = bstate)
# Set the state of the comm port entry. Enable when not connected,
# Disabled when the bot is connected.
def ConnCtrlsState (self, bstate):
self.connectButton.config (state = bstate)
self.comm_entry.config (state = bstate)
# Connect to the comm port typed in the comm entry field.
def SerialConnect (self):
try:
# Change the baud rate here if diffrent then 57600
self.ser = serial.Serial (self.comm_entry.get ( ), 57600)
except IOError:
tkMessageBox.showerror ("Invalid comm port", "Comm port not found.")
return
self.ConnCtrlsState (DISABLED)
self.CtrlButtonsState (NORMAL)
self.message.set ("SerialConnect")
self.connected = True
time.sleep (3) # Dwell a bit for the connection to happen
# Disconnect from the bot (close the comm port).
def SerialDisconnect (self):
try:
# Send a Halt command just in case the bot is still moving.
self.send_cmd ('h', "Halt!")
time.sleep (1)
self.ser.close ( )
except IOError:
print "Could not close port..."
self.message.set ("SerialDisconnect")
self.ConnCtrlsState (NORMAL)
self.CtrlButtonsState (DISABLED)
self.connected = False
time.sleep (2) # Dwell a bit for the disconnection to happen
# Send the command to the open comm port
def send_cmd (self, action, msg):
if self.connected == True:
for val in action:
self.ser.write (val)
self.ser.flush ( )
self.message.set (msg)
# Send the bot a turn-left command.
def TurnLeft (self, event = None):
self.send_cmd ('a', "Left")
# Send the bot a turn-left-up command.
def TurnLeft45 (self, event = None):
self.send_cmd ('q', "Left45")
# Send the bot a turn-right command.
def TurnRight (self, event = None):
self.send_cmd ('s', "Right")
# Send the bot a turn-right-up command.
def TurnRight45 (self, event = None):
self.send_cmd ('e', "Right45")
# Send the bot a Forward command.
def Forward (self, event = None):
self.send_cmd ('w', "Up")
# Send the bot a Reverse command.
def Reverse (self, event = None):
self.send_cmd ('z', "Down")
# Send the bot a Halt command.
def Halt (self, event = None):
self.send_cmd ('h', "Halt!")
# Set the gripper (X).
def GripperX (self, event = None):
# Read the slider control and send the value to the bot controller
# Note: 0 is all the way closed and 180 is all the way open
grp_change = ('>', chr (self.grip_horz.get ( )), chr (255))
self.send_cmd (grp_change, "Gripper X")
# Set the gripper Y.
def GripperY (self, event = None):
# Read the slider control and send the value to the bot controller
# Note: 0 is all the way up and 180 is all the way down
grp_change = ('^', chr (self.grip_vert.get ( )), chr (255))
self.send_cmd (grp_change, "Gripper Y")
# Set the gripper to the "home" position.
def GripperHome (self, event = None):
self.send_cmd (('c', chr (255)), "Gripper Home")
def __init__ (self, master = None):
Frame.__init__ (self, master)
self.pack ( )
self.createWidgets ( )
self.CtrlButtonsState (DISABLED)
# Kick off the GUI (Tk) then size and title the app window
def main ( ):
root = Tk ( )
root.geometry ("450x350")
root.wm_title ("Rover Control Center (RCC)")
app = App (master = root)
app.mainloop ( )
if __name__ == '__main__':
main ( )
<br> Code Listing for the Robot Driver
/*
Bluetooth Rover.
Goal in life...
Follows your commands sent magically though the air! Or from USB :)
Written by Scott Beasley - 2015
Free to use or modify. Enjoy.
*/
/*
Uses the L9110S library. It works with the L9110S h-bridge.
Download from https://github.com/jscottb/L9110Driver or clone the zip from
https://github.com to remove the '-master' from the archive file name to add
the library
*/
#include <L9110Driver.h>
#include <Servo.h>
#define SERVO_Y 2 // Pin gripper Y servo
#define SERVO_CLAW 7 // Pin gripper Gripper servo
#define pinAIN1 5 // define I1 interface
#define pinAIN2 6 // define I2 interface
#define pinBIN1 3 // define I3 interface
#define pinBIN2 11 // define I4 interface
// Speed defines
#define MAXFORWARDSPEED 225 // Max speed we want moving forward
#define MAXBACKWARDSPEED 225 // Max reverse speed
#define TOPSPEED 255 // Used to help turn better on carpet and rougher surfaces.
// Various time delays used for driving and servo
#define TURNDELAY 475
#define TURNDELAY45 235
#define BACKUPDELAY 400
#define SERVOMOVEDELAY 200
#define SERVOSEARCHDELAY 85
/*
Globals area.
*/
// Create the motor, servo objects to interface with
L9110_Motor motor_left (pinAIN1, pinAIN2); // Create Left motor object
L9110_Motor motor_right (pinBIN1, pinBIN2); // Create Right motor object
Servo grip_y_servo; // Create a servo object for the gripper Y axis
Servo grip_servo; // Create a servo object for the gripper claw
void setup ( )
{
// Change the baud rate here if different then 57600
Serial.begin (57600);
grip_y_servo.attach (SERVO_Y); // Attach the servo SERVO_LR
grip_y_servo.write (90);
grip_servo.attach (SERVO_CLAW); // Attach the servo SERVO_LR
grip_servo.write (90);
delay (500);
}
void loop ( )
{
byte command = 0, val = 0;
if (Serial.available ( ) > 0) {
// read the incoming command byte
command = Serial.read ( );
}
switch (command)
{
case 'w':
go_forward ( );
//Serial.println ("Going Forward");
break;
case 'z':
go_backward ( );
//Serial.println ("Going Backwards");
break;
case 'a':
go_left ( );
delay (TURNDELAY);
halt ( );
//Serial.println ("Turning Left");
break;
case 's':
go_right ( );
delay (TURNDELAY);
halt ( );
//Serial.println ("Turning Right");
break;
case 'q':
go_left ( );
delay (TURNDELAY45);
halt ( );
//Serial.println ("Turning Left");
break;
case 'e':
go_right ( );
delay (TURNDELAY45);
halt ( );
//Serial.println ("Turning Right");
break;
case 'h':
halt ( );
//Serial.println ("Halting");
break;
case '>':
// Gripper X move sends servo set value
val = Serial.read ( );
// We limit the value to real movement limits of the setup
grip_servo.write (constrain (val, 64, 179));
//Serial.println ("GripperX");
break;
case '^':
// Gripper Y move sends servo set value
val = Serial.read ( );
// We limit the value to real movement limits of the setup
grip_y_servo.write (constrain (val, 53, 179));
//Serial.println ("GripperY");
break;
case 'c':
// We limit the value to real movement limits of the setup
grip_y_servo.write (90);
grip_servo.write (90);
//Serial.println ("GripperHome");
break;
case 255: // Sent after all gripper commands
Serial.flush ( );
break;
}
Serial.flush ( );
delay(125);
}
void go_forward ( )
{
//Serial.println ("Going forward...");
// Ramp the motors up to the speed.
// Help with spinning out on some surfaces and ware and tare on the GM's
ramp_it (MAXFORWARDSPEED, FORWARD, FORWARD);
// Set to all of the set speed just incase the ramp last vat was not all of
// it.
motor_left.setSpeed (MAXFORWARDSPEED);
motor_right.setSpeed (MAXFORWARDSPEED);
motor_left.run (FORWARD|RELEASE);
motor_right.run (FORWARD|RELEASE);
}
void go_backward ( )
{
//Serial.println ("Going backward...");
// Ramp the motors up to the speed.
// Help with spinning out on some surfaces and ware and tare on the GM's
ramp_it (MAXBACKWARDSPEED, BACKWARD, BACKWARD);
// Set to all of the set speed just incase the ramp last vat was not all of
// it.
motor_left.setSpeed (MAXBACKWARDSPEED);
motor_right.setSpeed (MAXBACKWARDSPEED);
motor_left.run (BACKWARD|RELEASE);
motor_right.run (BACKWARD|RELEASE);
}
void go_left ( )
{
//Serial.println ("Going left...");
// Ramp the motors up to the speed.
// Help with spinning out on some surfaces and ware and tare on the GM's
ramp_it (TOPSPEED, BACKWARD, FORWARD);
// Set to all of the set speed just incase the ramp last vat was not all of
// it.
motor_left.setSpeed (TOPSPEED);
motor_right.setSpeed (TOPSPEED);
motor_left.run (BACKWARD|RELEASE);
motor_right.run (FORWARD|RELEASE);
}
void go_right ( )
{
//Serial.println ("Going right...");
// Ramp the motors up to the speed.
// Help with spinning out on some surfaces and ware and tare on the GM's
ramp_it (TOPSPEED, FORWARD, BACKWARD);
// Set to all of the set speed just incase the ramp last vat was not all of
// it.
motor_left.setSpeed (TOPSPEED);
motor_right.setSpeed (TOPSPEED);
motor_left.run (FORWARD|RELEASE);
motor_right.run (BACKWARD|RELEASE);
}
void halt ( )
{
//Serial.println ("Halt!");
//ramp_it (0, BRAKE, BRAKE);
motor_left.setSpeed (0);
motor_right.setSpeed (0);
motor_left.run (BRAKE);
motor_right.run (BRAKE);
}
void ramp_it (uint8_t speed, uint8_t lf_dir, uint8_t rt_dir)
{
uint8_t ramp_val = 0, step_val = 0;
step_val = abs (speed / 4);
if (!speed)
step_val = -step_val;
for (uint8_t i = 0; i < 4; i++) {
ramp_val += step_val;
motor_left.setSpeed (ramp_val);
motor_right.setSpeed (ramp_val);
motor_left.run (lf_dir|RELEASE);
motor_right.run (rt_dir|RELEASE);
delay (25);
}
}