Sabertooth.cpp

00001 /**
00002  * @author Birdy
00003  *
00004  * @section LICENSE
00005  *
00006  * Copyright (c) 2010 ARM Limited
00007  *
00008  * Permission is hereby granted, free of charge, to any person obtaining a copy
00009  * of this software and associated documentation files (the "Software"), to deal
00010  * in the Software without restriction, including without limitation the rights
00011  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00012  * copies of the Software, and to permit persons to whom the Software is
00013  * furnished to do so, subject to the following conditions:
00014  *
00015  * The above copyright notice and this permission notice shall be included in
00016  * all copies or substantial portions of the Software.
00017  *
00018  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00019  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00020  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00021  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00022  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00023  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00024  * THE SOFTWARE.
00025  *
00026  * @section DESCRIPTION
00027  * 
00028  * Sabertooth motor drivers  
00029  *
00030  * Website:
00031  * http://www.dimensionengineering.com/
00032  * 
00033  */
00034 
00035 /**
00036  * Includes
00037  */
00038 #include "Sabertooth.h"
00039 
00040 Sabertooth::Sabertooth(PinName Tx, int address, int baudrate) : _sabertoothserial(Tx, NC) {
00041   
00042     _Address = address;
00043     _sabertoothserial.baud(baudrate);
00044     // Initialize Current Speed
00045     _CurrentSpeed[A] = 0;
00046     _CurrentSpeed[B] = 0;
00047 
00048 }
00049 
00050 void Sabertooth::InitializeCom(void){
00051     wait(1); // wait for sabertooth to initialized
00052     _sabertoothserial.putc(START_CHARACTER);
00053 }
00054 
00055 // Valid speed is between -127 and +127
00056 void Sabertooth::SetSpeedMotorA(int speed)
00057 {
00058     SetMotorSpeed(A, speed);
00059 }
00060 
00061 // Valid speed is between -127 and +127
00062 void Sabertooth::SetSpeedMotorB(int speed)
00063 {
00064     SetMotorSpeed(B, speed);
00065 }
00066 
00067 
00068 void Sabertooth::SetMotorSpeed(int motor,int speed){
00069    
00070     speed = ClipSpeed(speed);
00071     _CurrentSpeed[motor] = speed;
00072     int motorCommandStart = (motor == 0 ? 0 : 4);
00073     if (speed >= 0)
00074     {
00075         // forward
00076         Send(motorCommandStart + 0, speed);
00077     }
00078     else
00079     {
00080         // backwards
00081         Send(motorCommandStart + 1, -speed);
00082     }
00083 
00084 }
00085 
00086 // Valid speed is between -127 and +127
00087 int Sabertooth::GetCommandedSpeedA()
00088 {
00089     return _CurrentSpeed[A];
00090 }
00091 
00092     // Valid speed is between -127 and +127
00093 int Sabertooth::GetCommandedSpeedB()
00094 {
00095     return _CurrentSpeed[B];
00096 }
00097 
00098 void Sabertooth::ShutdownMotors(void){
00099     _CurrentSpeed[A] = 0;
00100     _CurrentSpeed[B] = 0;
00101     SetSpeedMotorA(_CurrentSpeed[A]);
00102     SetSpeedMotorB(_CurrentSpeed[B]);
00103 }
00104 
00105 
00106 int Sabertooth::ClipSpeed(int speed)
00107 {
00108     if (speed < -127)
00109     {
00110         return -127;
00111     }
00112     if (speed > 127)
00113     {
00114         return 127;
00115     }
00116     return speed;
00117 }
00118 
00119 
00120 void Sabertooth::Send(int command, int value)
00121 {
00122     int tmp;
00123     _sabertoothserial.putc(_Address);
00124     _sabertoothserial.putc(command);
00125     _sabertoothserial.putc(value);
00126     tmp = _Address+command+value;
00127     tmp = tmp & 0x7F;
00128     _sabertoothserial.putc(tmp);
00129 }