Revision 10:294b9adbc9d3, committed 2012-04-28

Comitter:

narshu

Date:

Sat Apr 28 18:10:55 2012 +0000

Parent:

9:377560539b74

Child:

11:ea2112ae3c4a

Child:

12:2981367c63a0

Commit message:

Moved motion stuff to own file, and created class

Changed in this revision

--- a/globals.h	Sat Apr 28 17:21:24 2012 +0000
+++ b/globals.h	Sat Apr 28 18:10:55 2012 +0000
@@ -30,8 +30,8 @@
 //Robot movement constants
 const float fwdvarperunit = 0.008; //1 std dev = 7% //NEEDS TO BE MEASURED AGAIN!
 const float varperang = 0.005; //around 1 degree stddev per 180 turn
-const float xyvarpertime = 0.0005; //(very poorly) accounts for hitting things
-const float angvarpertime = 0.001;
+const float xyvarpertime = 0;//0.0005; //(very poorly) accounts for hitting things
+const float angvarpertime = 0;//0.001;
 
 //sonar constants
 static const float sonarvariance = 0.005;
@@ -61,15 +61,23 @@
 #define RELI_BOUND_LOW          4
 #define RELI_BOUND_HIGH         25
 
-// Localization estimate tolerences
+// Movement target tolerances
 #define POSITION_TOR            50
-#define ANGLE_TOR               0.15
+#define ANGLE_TOR               0.1
 
 // motion control
-#define MOVE_SPEED              50
+#define MOVE_SPEED              30
 #define MAX_STEP_RATIO          0.10 //maximum change in the speed
 //#define TRACK_RATE              10       // +- rate for each wheel when tracking
 
+#ifdef ROBOT_PRIMARY
+#define FWD_MOVE_P 1
+#define SPIN_MOVE_P 1.3
+#else
+#define FWD_MOVE_P 3.2
+#define SPIN_MOVE_P 4
+#endif
+
 // Task suspend periods
 #define IR_TURRET_PERIOD        200
 #define MOTION_UPDATE_PERIOD    20

--- a/main.cpp	Sat Apr 28 17:21:24 2012 +0000
+++ b/main.cpp	Sat Apr 28 18:10:55 2012 +0000
@@ -7,6 +7,7 @@
 #include "math.h"
 #include "system.h"
 #include "geometryfuncs.h"
+#include "motion.h"
 #include "ai.h"
 #include "ui.h"
 
@@ -17,10 +18,9 @@
 
 Motors motors;
 UI ui;
-
-
 Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11);
 AI ai;
+Motion motion(motors, ai, kalman);
 
 //TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus)
 //NOTE! Recieving data with RF12B now DISABLED due to interferance with rtos!
@@ -43,7 +43,7 @@
     //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
     //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
     
-    Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024);
+    //Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024);
     //Motion_Thread_Ptr = &thr_motion;
 
     //measure cpu usage. output updated once per second to symbol cpupercent
@@ -68,7 +68,7 @@
         }
         
         // do nothing
-        Thread::wait(osWaitForever);
+        //Thread::wait(osWaitForever);
     }
 }
 
@@ -151,7 +151,6 @@
 }
 
 
-
 void vPrintState(void const *argument) {
     float state[3];
     float SonarMeasures[3];
@@ -177,101 +176,3 @@
         Thread::wait(100);
     }
 }
-
-// motion control thread ------------------------
-void motion_thread(void const *argument) {
-    motors.resetEncoders();
-    //motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2);
-    //Thread::wait(1000);
-    motors.stop();
-    ai.thr_AI.signal_set(0x01);
-
-    float currX, currY,currTheta;
-    float speedL,speedR;
-    float diffDir,diffSpeed;
-    float lastdiffSpeed = 0;
-
-    while (1) {
-        if (ai.flag_terminate) {
-            terminate();
-        }
-
-        // get kalman localization estimate ------------------------
-        kalman.statelock.lock();
-        currX = kalman.X(0)*1000.0f;
-        currY = kalman.X(1)*1000.0f;
-        currTheta = kalman.X(2);
-        kalman.statelock.unlock();
-
-
-        // check if target reached ----------------------------------
-        if (  ( abs(currX - ai.target.x) < POSITION_TOR )
-                &&( abs(currY - ai.target.y) < POSITION_TOR )
-           ) {
-
-            diffDir = rectifyAng(currTheta - ai.target.theta);
-            diffSpeed = diffDir / PI;
-
-            if (abs(diffDir) > ANGLE_TOR) {
-                if (abs(diffSpeed) < 0.5) {
-                    diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
-                }
-                motors.setSpeed( int(diffSpeed*MOVE_SPEED),  -int(diffSpeed*MOVE_SPEED));
-
-
-            } else {
-                motors.stop();
-                Thread::wait(4000);
-                ai.thr_AI.signal_set(0x01);
-            }
-        }
-
-        // adjust motion to reach target ----------------------------
-        else {
-
-            // calc direction to target
-            float targetDir = atan2(ai.target.y - currY, ai.target.x - currX);
-            if (!ai.target.facing)
-                targetDir = PI - targetDir;
-
-
-            diffDir = rectifyAng(currTheta - targetDir);
-            diffSpeed = diffDir / PI;
-
-            if (abs(diffDir) > ANGLE_TOR*2) {
-                if (abs(diffSpeed) < 0.5) {
-                    diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
-                }
-                motors.setSpeed( int(diffSpeed*MOVE_SPEED),  -int(diffSpeed*MOVE_SPEED));
-            } else {
-
-
-                if (abs(diffSpeed-lastdiffSpeed) > MAX_STEP_RATIO  ) {
-                    if (diffSpeed-lastdiffSpeed >= 0) {
-                        diffSpeed = lastdiffSpeed + MAX_STEP_RATIO;
-                    } else {
-                        diffSpeed = lastdiffSpeed - MAX_STEP_RATIO;
-                    }
-                }
-                lastdiffSpeed = diffSpeed;
-
-                // calculte the motor speeds
-                if (diffSpeed <= 0) {
-                    speedL = (1-2*abs(diffSpeed))*MOVE_SPEED;
-                    speedR = MOVE_SPEED;
-
-                } else {
-                    speedR = (1-2*abs(diffSpeed))*MOVE_SPEED;
-                    speedL = MOVE_SPEED;
-                }
-                if (ai.target.facing)
-                    motors.setSpeed( int(speedL),  int(speedR));
-                else
-                    motors.setSpeed( -int(speedL),  -int(speedR));
-            }
-        }
-
-        // wait
-        Thread::wait(MOTION_UPDATE_PERIOD);
-    }
-}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motion.cpp	Sat Apr 28 18:10:55 2012 +0000
@@ -0,0 +1,129 @@
+#include "motion.h"
+#include "geometryfuncs.h"
+
+Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
+    motors(motorsin),
+    ai(aiin),
+    kalman(kalmanin) { }
+
+// motion control thread ------------------------
+void Motion::motion_thread() {
+    motors.resetEncoders();
+    motors.stop();
+    ai.thr_AI.signal_set(0x01);
+
+    //PID declare
+    PID PIDControllerMotorTheta2(FWD_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f);     //Going forward
+    PID PIDControllerMotorTheta(SPIN_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f);    //Spinning on the spot
+
+    //PID Initialisation
+    PIDControllerMotorTheta2.setMode(MANUAL_MODE);
+    PIDControllerMotorTheta.setMode(MANUAL_MODE);
+
+    PIDControllerMotorTheta2.setBias(0);
+    PIDControllerMotorTheta.setBias(0);
+
+    PIDControllerMotorTheta2.setOutputLimits(-1, 1);
+    PIDControllerMotorTheta.setOutputLimits(-1, 1);
+
+    PIDControllerMotorTheta2.setInputLimits(-PI, PI);
+    PIDControllerMotorTheta.setInputLimits(-PI, PI);
+
+    PIDControllerMotorTheta.setSetPoint(0);
+    PIDControllerMotorTheta2.setSetPoint(0);
+
+    float currX, currY,currTheta;
+    float speedL,speedR;
+    float diffDir;
+    
+    while (1) {
+        if (ai.flag_terminate) {
+            terminate();
+        }
+
+        // get kalman localization estimate ------------------------
+        kalman.statelock.lock();
+        currX = kalman.X(0)*1000.0f;
+        currY = kalman.X(1)*1000.0f;
+        currTheta = kalman.X(2);
+        kalman.statelock.unlock();
+        
+       
+       /* 
+        if (pc.readable() == 1) {
+            pc.scanf("%f", &cmd);
+            //Tune PID referece
+               PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
+        }
+        */  
+
+        // check if target reached ----------------------------------
+        if (  ( abs(currX - ai.target.x) < POSITION_TOR )
+                &&( abs(currY - ai.target.y) < POSITION_TOR )
+           ) {
+
+            diffDir = rectifyAng(currTheta - ai.target.theta);
+            //diffSpeed = diffDir / PI;
+
+            PIDControllerMotorTheta.setProcessValue(diffDir);
+
+            if (abs(diffDir) > ANGLE_TOR) {
+            
+                float tempPidVar = PIDControllerMotorTheta.compute();
+                motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
+
+            } else {
+                motors.stop();
+                Thread::wait(4000);
+                ai.thr_AI.signal_set(0x01);
+            }
+        }
+
+        // adjust motion to reach target ----------------------------
+        else {
+
+            // calc direction to target
+            float targetDir = atan2(ai.target.y - currY, ai.target.x - currX);
+            if (!ai.target.facing) targetDir =  targetDir - PI;
+
+            //Angle differene in -PI to PI
+            diffDir = rectifyAng(currTheta - targetDir);
+
+            //Set PID process variable
+            PIDControllerMotorTheta.setProcessValue(diffDir);
+            PIDControllerMotorTheta2.setProcessValue(diffDir);
+
+            //if diffDIr is neg, spin right
+            //if diffDir is pos, spin left
+
+            if (abs(diffDir) > ANGLE_TOR*4) {   //roughly 32 degrees
+                //ANGLE_TOR*4
+                float tempPidVar = PIDControllerMotorTheta.compute();
+                motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
+                // pc.printf("spin,%f\n",diffDir);
+
+            } else {
+             
+             float tempPidVar = PIDControllerMotorTheta2.compute();
+             //pc.printf("turn,%f\n",diffDir);
+                // calculte the motor speeds
+                if (tempPidVar >= 0) {
+                    //turn left
+                    speedL = (1-abs(tempPidVar))*MOVE_SPEED;
+                    speedR = MOVE_SPEED;
+
+                } else {
+                    //turn right
+                    speedR = (1-abs(tempPidVar))*MOVE_SPEED;
+                    speedL = MOVE_SPEED;
+                }
+                if (ai.target.facing) motors.setSpeed( int(speedL),  int(speedR));
+                else motors.setSpeed( -int(speedR),  -int(speedL));
+                
+            }
+        }
+
+        // wait
+        Thread::wait(MOTION_UPDATE_PERIOD);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motion.h	Sat Apr 28 18:10:55 2012 +0000
@@ -0,0 +1,17 @@
+#include "motors.h"
+#include "ai.h"
+#include "Kalman.h"
+
+class Motion {
+public:
+    Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin);
+
+private:
+    Motors& motors;
+    AI& ai;
+    Kalman& kalman;
+    
+    void motion_thread();
+    static void mtwrapper(void const *arg){ ((Motion*)arg)->motion_thread(); }
+
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motors/motors.cpp	Sat Apr 28 18:10:55 2012 +0000
@@ -0,0 +1,384 @@
+/**********************************************************
+ * Motors.cpp
+ *
+ * This is a motor control library for the UK1122 L298N based motor controller.
+ * Includes PID controller to control motors speeds
+ *
+ *        Author: Crispian Poon
+ *         Email: pooncg@gmail.com
+ *       Purpose: Eurobot 2012 - ICRS Imperial College London
+ *          Date: 4th April 2012
+ *       Version: v0.12
+ **********************************************************/
+
+#include "mbed.h"
+#include "motors.h"
+#include "QEI.h"    //quadrature encoder library
+#include "globals.h"
+#include "TSH.h"
+
+//*************************************************************************************
+//  Constructor
+//*************************************************************************************
+Motors::Motors():
+        Encoder1 (p30, p29, NC, 1856 ,QEI::X4_ENCODING),    //connects to motor1 quadracture encoders
+        Encoder2 (p27, p28, NC, 1856 ,QEI::X4_ENCODING),    //connects to motor2 quadracture encoders
+        Motor1A(p17), Motor1B(p18), Motor2A(p19), Motor2B(p13), //connects to direction pins
+        Motor1Enable(p25), Motor2Enable(p26),   //Connects to control board enable pins to control motors speeds. PWM pins. Remember enable must be set before the direction pins changed!!
+        PIDControllerMotor1(3.5, 0.5, 0, 0.010), PIDControllerMotor2(3.5, 0.5, 0, 0.010) {
+
+//Initialise PID controllers
+    PIDControllerMotor1.setMode(MANUAL_MODE);
+    PIDControllerMotor2.setMode(MANUAL_MODE);
+    PIDControllerMotor1.setBias(-16);
+    PIDControllerMotor2.setBias(-16);
+    PIDControllerMotor1.setOutputLimits(-127, 127);
+    PIDControllerMotor2.setOutputLimits(-127, 127);
+    PIDControllerMotor1.setInputLimits(-102, 102);
+    PIDControllerMotor2.setInputLimits(-102, 102);
+    _lastEncoder1 = 0;
+    _lastEncoder2 = 0;
+    
+    //speed regulator task using PID. Run every 10ms.
+    _ticker.attach(this, &Motors::speedRegulatorTask, 0.01);
+
+};
+
+
+//*************************************************************************************
+//  Public functions
+//*************************************************************************************
+
+//*********************************************
+//
+//  @Description    speed regulator task using PID. Run every 10ms.
+//
+//*********************************************
+void Motors::speedRegulatorTask() {
+
+    int latestMotor1Speed = 0;
+    int latestMotor2Speed = 0;
+    int computedSpeed1 = 0;
+    int computedSpeed2 = 0;
+
+    //acceleration control
+    if (accelerationRegister == 1) {
+        if (_accelerationSpeed1 != 0) {
+
+            if (abs(_motorSpeed1) < abs(_accelerationSpeed1)) {
+                _motorSpeed1 += getSignOfInt(_accelerationSpeed1);
+            } else if (abs(_motorSpeed1) > abs(_accelerationSpeed1)) {
+                _motorSpeed1 = _accelerationSpeed1;
+            }
+        }
+        if (_accelerationSpeed2 != 0) {
+            if (abs(_motorSpeed2) < abs(_accelerationSpeed2)) {
+                _motorSpeed2 += getSignOfInt(_accelerationSpeed2);
+            } else if (abs(_motorSpeed2) > abs(_accelerationSpeed2)) {
+                _motorSpeed2 = _accelerationSpeed2;
+            }
+        }
+    }
+
+
+    //MOTOR 1 PID
+    latestMotor1Speed = getEncoder1() - _lastEncoder1;  //motor1 encoder change
+    //PID setpoints for 50ms interval.
+    if (_motorSpeed1 == 0) {
+        PIDControllerMotor1.setSetPoint(0);
+    } else {
+        PIDControllerMotor1.setSetPoint((int)(102*((float)_motorSpeed1/127)));
+    }
+    //Process value
+    PIDControllerMotor1.setProcessValue(latestMotor1Speed);
+    //PID Compute
+    computedSpeed1 = (int)PIDControllerMotor1.compute();
+
+
+
+    //MOTOR 2 PID
+    latestMotor2Speed = getEncoder2() - _lastEncoder2; //motor2 encoder change
+    //PID setpoints for 50ms interval.
+    if (_motorSpeed2 == 0) {
+        PIDControllerMotor2.setSetPoint(0);
+    } else {
+        PIDControllerMotor2.setSetPoint((int)(102*((float)_motorSpeed2/127)));
+    }
+    //Process value
+    PIDControllerMotor2.setProcessValue(latestMotor2Speed);
+    //PID Compute
+    computedSpeed2 = (int)PIDControllerMotor2.compute();
+
+
+
+    //debug variables
+    _debug1 = latestMotor1Speed;
+    _debug2 = computedSpeed1;
+
+
+
+    //Set motors speed
+    _setSpeed(computedSpeed1, computedSpeed2);
+
+}
+
+//*********************************************
+//
+//  @Description    External set speed function for both motors
+//  @Parameter  [speed]   ranges from -127 (revse motor) to 127 (forward motor)
+//
+//*********************************************
+void Motors::setSpeed(int speed) {
+    setSpeed(speed, speed);
+}
+
+//*********************************************
+//
+//  @Description    External set speed function. Relies on the speedRegulatorTask to change speed.
+//  @Parameters [speed1]  min -127 (reverse motor), max 127 (forward motor)
+//  @Parameters [speed2]  min -127 (reverse motor), max 127 (forward motor)
+//
+//*********************************************
+void Motors::setSpeed(int speed1, int speed2) {
+    //set global motor values
+    _motorSpeed1 = speed1;
+    _motorSpeed2 = speed2;
+    _lastEncoder1 = getEncoder1();
+    _lastEncoder2 = getEncoder2();
+
+    //acceleration control
+    if (accelerationRegister == 1) {
+        //target accelerated speed
+        _accelerationSpeed1 = speed1;
+        _accelerationSpeed2 = speed2;
+
+        //current speed
+        _motorSpeed1 = 0;
+        _motorSpeed2 = 0;
+    }
+}
+
+//*********************************************
+//
+//  @Description    stops motors
+//
+//*********************************************
+void  Motors::stop()
+{
+    setSpeed(0);
+}
+
+//*********************************************
+//
+//  @Description    resets motor1 and motor encoders
+//
+//*********************************************
+void Motors::resetEncoders() {
+    Encoder1.reset();
+    Encoder2.reset();
+}
+
+//*********************************************
+//
+//  @Description    gets motor1 encoder
+//  @returns motor1 encoder counts
+//
+//*********************************************
+int Motors::getEncoder1() {
+    return  Encoder1.getPulses();
+}
+
+//*********************************************
+//
+//  @Description    gets motor2 encoder
+//  @returns motor2 encoder counts
+//
+//*********************************************
+int Motors::getEncoder2() {
+    return  Encoder2.getPulses();
+}
+
+//*********************************************
+//
+//  @Description   converts encoder counts to distance in mm
+//  @Parameters [encoder]   (int) encoder counts
+//  @returns distance in mm
+//
+//*********************************************
+int Motors::encoderToDistance(int encoder) {
+    return int((float(encoder) / float(encoderRevCount)) * wheelmm);
+}
+
+//*********************************************
+//
+//  @Description   converts distance in mm to encoder counts
+//  @Parameters [distance]   (int) distance in mm
+//  @returns encoder counts
+//
+//*********************************************
+int Motors::distanceToEncoder(int distance) {
+    return int((float(distance) / float(wheelmm)) * encoderRevCount);
+}
+
+//*********************************************
+//
+//  @Description   number sign indicator. determines if number is positive or negative.
+//  @Parameters [direction]   (int) a number
+//  @returns -1 if negative, 1 if positive
+//
+//*********************************************
+int Motors::getSignOfInt(int direction) {
+
+    if (direction > 0) {
+        return 1;
+    } else if (direction < 0) {
+        return -1;
+    }
+    return -1;
+}
+
+//*********************************************
+//Start of quarantined functions
+
+void Motors::move(int distance, int speed) {
+    //resetEncoders(); TODO use kalman as feedback instead!
+
+    int tempEndEncoder = 0;
+    int startEncoderCount = 0;
+
+    tempEndEncoder = distanceToEncoder(abs(distance));
+    startEncoderCount = getEncoder1();
+
+    setSpeed(getSignOfInt(distance) * speed);
+
+    while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
+        setSpeed(getSignOfInt(distance) * speed);
+    }
+
+    //resetEncoders();
+    setSpeed(0);
+}
+
+void Motors::turn(int angle, int speed) {
+    //resetEncoders(); TODO use kalman as feedback instead!
+    int tempDistance = int((float(angle) / 360) * float(robotCircumference));
+    int tempEndEncoder = 0;
+    int startEncoderCount = 0;
+
+    tempEndEncoder = distanceToEncoder(abs(tempDistance));
+    startEncoderCount = getEncoder1();
+    setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed);
+
+    while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
+        setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed);
+
+    }
+
+    //resetEncoders();
+    setSpeed(0);
+}
+
+//Start of quarantined functions
+//*********************************************
+
+
+//*************************************************************************************
+//  Private functions
+//*************************************************************************************
+
+//*********************************************
+//
+//  @Description    internal set speed function
+//  @Parameters speed1  min -127, max 127
+//  @Parameters speed2  min -127, max 127
+//
+//*********************************************
+void Motors::_setSpeed(int speed1, int speed2) {
+
+//set global encoder values
+    _lastEncoder1 = getEncoder1();
+    _lastEncoder2 = getEncoder2();
+
+//Speed ranges from -127 to 127
+    if (speed1 > 0) {
+        //Motor1 forwards
+        Motor1Enable = (float)speed1/127;
+        Motor1A = 1;
+        Motor1B = 0;
+        //pwm the h bridge driver range 0 to 1 type float.
+
+    } else if (speed1 < 0) {
+        //Motor1 backwards
+        Motor1Enable = (float)abs(speed1)/127;
+        Motor1A = 0;
+        Motor1B = 1;
+
+    } else if (speed1 ==0) {
+        _stop(1,0);
+    }
+
+    if (speed2 > 0) {
+        //Motor2 forwards
+        Motor2Enable = (float)speed2/127;
+
+        Motor2A = 1;
+        Motor2B = 0;
+
+    } else if (speed2 < 0) {
+        //Motor2 backwards
+        Motor2Enable = (float)abs(speed2)/127;
+        Motor2A = 0;
+        Motor2B = 1;
+    } else if (speed2 == 0) {
+        _stop(0,1);
+    }
+
+}
+
+
+//*********************************************
+//
+//  @Description    stop command for both motors
+//
+//*********************************************
+void Motors::_stop() {
+    _stop(1,1);
+}
+
+//*********************************************
+//
+//  @Description    stop command for individual motors
+//  @Parameter  [motor1]  stops motor1. =1 is stop. =0 do nothing
+//  @Parameter  [motor2]  stops motor2. =1 is stop. =0 do nothing
+//
+//*********************************************
+void Motors::_stop(int motor1, int motor2) {
+    if (motor1 == 1) {
+        Motor1Enable = 1;
+        Motor1A = 0;
+        Motor1B = 0;
+    }
+
+    if (motor2 == 1) {
+        Motor2Enable = 1;
+        Motor2A = 0;
+        Motor2B = 0;
+    }
+
+}
+
+//*************************************************************************************
+//  Redundant functions
+//*************************************************************************************
+
+//Redudant
+void Motors::setMode(int mode) {
+}
+
+//Redudant
+void Motors::sendCommand(char command) {
+}
+
+//Redudant
+void Motors::sendCommand(char command1, char command2 ) {
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motors/motors.h	Sat Apr 28 18:10:55 2012 +0000
@@ -0,0 +1,62 @@
+#ifndef MOTORS_H
+#define MOTORS_H
+
+#include "mbed.h"
+#include "QEI.h"
+#include "PID.h"
+#include "TSH.h"
+
+class Motors {
+
+public:
+    Motors();
+    Motors(TSI2C &i2cin);
+
+    //Functions declaration
+    void resetEncoders();
+    int getEncoder1();
+    int getEncoder2();
+    void move(int distance, int speed);
+    void turn(int angle, int speed);
+    int getSignOfInt(int direction);
+    void setSpeed(int speed);
+    void setSpeed(int speed1, int speed2);
+    void stop();
+    void setMode(int mode);
+    int encoderToDistance(int encoder);
+    int distanceToEncoder(int distance);
+    void sendCommand(char command);
+    void sendCommand(char command1, char command2 );
+    void speedRegulatorTask();
+    float _debug1;
+    float _debug2;
+    int accelerationRegister;   //turns on acceleration control
+
+private:
+
+
+    void _setSpeed(int speed1, int speed2);
+    void _stop();
+    void _stop(int motor1, int motor2);
+    QEI Encoder1;
+    QEI Encoder2;
+    DigitalOut Motor1A;
+    DigitalOut Motor1B;
+    DigitalOut Motor2A;
+    DigitalOut Motor2B;
+    PwmOut Motor1Enable;
+    PwmOut Motor2Enable;
+    int _motorSpeed1;
+    int _motorSpeed2;
+    PID PIDControllerMotor1;
+    PID PIDControllerMotor2;
+    int _lastEncoder1;
+    int _lastEncoder2;
+    int _pidDataBufferIndex;
+    int _accelerationSpeed1;
+    int _accelerationSpeed2;
+    Ticker _ticker;
+
+};
+
+#endif