Diff: Robot.cpp

Revision:

0:fc95840345e8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.cpp	Tue Oct 12 20:51:08 2010 +0000
@@ -0,0 +1,199 @@
+#include "Robot.h"
+#include "mbed.h"
+
+// This library was designed simplify operation of Stinger robot only with MBED and two H-Bridges.
+// Is is still in development.
+// Class Robot integrated QEI, PID and Motor classes to work together through use of Ticker function to 
+// regularly poll QEI objects, adjust PID and Motor objects.
+// Right now all pins have been preassigned, later empty constructor will be added with individual pin assignments.
+//    Pinouts are:
+//    rightMotor = new Motor(p23, p6, p5); // pwm, fwd, rev
+//    leftMotor = new Motor(p22, p8, p7); // pwm, fwd, rev
+//    leftQei = new QEI(p29,  p30, NC, 624, QEI::X2_ENCODING);  //chanA, chanB, index, ppr
+//    rightQei = new QEI(p27, p28, NC, 624, QEI::X2_ENCODING);  //chanB, chanA, index, ppr
+//    leftPid = new PID(0.4312, 0.1, 0.0, RATE);  //Kc, Ti, Td
+//    rightPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td
+
+Robot::Robot() {
+    rightMotor = new Motor(p23, p6, p5); // pwm, fwd, rev
+    leftMotor = new Motor(p22, p8, p7); // pwm, fwd, rev
+    leftQei = new QEI(p29,  p30, NC, 624, QEI::X2_ENCODING);  //chanA, chanB, index, ppr
+    rightQei = new QEI(p27, p28, NC, 624, QEI::X2_ENCODING);  //chanB, chanA, index, ppr
+    leftPid = new PID(0.4312, 0.1, 0.0, RATE);  //Kc, Ti, Td
+    rightPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td
+    leftMotor->period(0.00005);  //Set motor PWM periods to 20KHz.
+    rightMotor->period(0.00005);
+    leftPid->setInputLimits(0, MAX_SPEED);
+    leftPid->setOutputLimits(0.3, 1);
+    leftPid->setMode(AUTO_MODE);
+    rightPid->setInputLimits(0, MAX_SPEED);
+    rightPid->setOutputLimits(0.3, 1);
+    rightPid->setMode(AUTO_MODE);
+    leftPulsesGoTo=rightPulsesGoTo=0;
+    ticker.attach(this, &Robot::Call, RATE); // the address of the function to be attached (call) and the interval (RATE)
+}
+
+Robot::~Robot() {
+    delete rightMotor;
+    delete leftMotor;
+    delete leftQei;
+    delete rightQei;
+    delete leftMotor;
+    delete rightMotor;
+    delete leftPid;
+    delete rightPid;
+    ticker.detach();
+}
+
+void Robot::MoveStraightPulses(float s, int pulses) {
+    leftQei->reset();
+    rightQei->reset();
+    leftPulses=rightPulses=0;
+    if (s>=0.0)leftDirection=rightDirection=1.0;
+    else leftDirection=rightDirection=-1.0;
+    leftSpeed=abs(s*MAX_SPEED);
+    rightSpeed=abs(s*MAX_SPEED);
+    leftPid->reset();
+    rightPid->reset();
+    leftPid->setSetPoint(leftSpeed);
+    rightPid->setSetPoint(rightSpeed);
+    leftPulsesGoTo=rightPulsesGoTo=pulses*2;
+    if (s==0)Stop();
+}
+
+void Robot::MoveStraightInches(float speed, float in){
+    MoveStraightPulses(speed, (int)(in*85.6144));
+}
+
+void Robot::MoveStraightRotations(float speed, float rotations){
+    MoveStraightPulses(speed, (int)(rotations*624.00));
+}
+
+void Robot::Stop() {
+    this->StopLeft();
+    this->StopRight();
+}
+
+void Robot::StopLeft() {
+    leftMotor->brake();
+    leftPulsesGoTo=0;
+    leftSpeed=0.0;
+    leftPulses=0;
+    leftQei->reset();
+    
+    leftPid->reset();
+    leftPid->setSetPoint(0);
+}
+
+void Robot::StopRight() {
+    rightMotor->brake();
+    rightPulsesGoTo=0;
+    rightSpeed=0.0;
+    rightPulses=0;
+    rightQei->reset();
+    rightPid->reset();
+    rightPid->setSetPoint(0);
+    
+
+}
+void Robot::RotateLeftWheel(float speed, float deg) {
+    leftPid->reset();
+    leftSpeed=abs(speed*MAX_SPEED);
+    if (speed>=0.0)leftDirection=1.0;
+    else leftDirection=-1.0;
+    leftPulsesGoTo=deg*624/360;
+    leftPid->setSetPoint(leftSpeed);
+}
+
+void Robot::RotateRightWheel(float speed, float deg) {
+    rightPid->reset();
+    rightSpeed=abs(speed*MAX_SPEED);
+    if (speed>=0.0)rightDirection=1.0;
+    else leftDirection=-1.0;
+    rightPulsesGoTo=deg*624/360;
+    rightPid->setSetPoint(rightSpeed);
+}
+
+void Robot::PivetLeft(float deg) {
+    if (deg<0.0) {
+        PivetRight(-deg);
+        return;
+    }
+    leftPid->reset();
+    rightPid->reset();
+    leftSpeed=abs(0.5*MAX_SPEED);
+    rightSpeed=abs(0.5*MAX_SPEED);
+    leftDirection=-1.0;
+    rightDirection=1.0;
+    leftPulsesGoTo=abs(deg*DPP);
+    rightPulsesGoTo=abs(deg*DPP);
+    leftPid->setSetPoint(leftSpeed);
+    rightPid->setSetPoint(rightSpeed);
+}
+
+void Robot::PivetRight(float deg) {
+    if (deg<0) {
+        PivetLeft(-deg);
+        return;
+    }
+    leftPid->reset();
+    rightPid->reset();
+    leftSpeed=abs(0.5*MAX_SPEED);
+    rightSpeed=abs(0.5*MAX_SPEED);
+    leftDirection=1.0;
+    rightDirection=-1.0;
+    leftPulsesGoTo=abs(deg*DPP);
+    rightPulsesGoTo=abs(deg*DPP);
+    leftPid->setSetPoint(leftSpeed);
+    rightPid->setSetPoint(rightSpeed);
+}
+
+int  Robot::IsBusy() {
+    if ((rightSpeed==0.0)&&(leftSpeed==0.0))return 0;
+    return 1;
+}
+
+void Robot::Call() {
+    float t;
+    if ((rightPulsesGoTo!=0)&&(rightSpeed>0)) {
+        if (rightPulses < rightPulsesGoTo) {
+            rightPulses = (int)rightDirection * rightQei->getPulses();
+            rightVelocity = (rightPulses - rightPrevPulses) / RATE;
+            rightPrevPulses = rightPulses;
+            t=rightPid->compute();
+            if(((rightPulsesGoTo-rightPulses)/rightVelocity)<1.0){              
+                rightPid->reset();
+                rightPid->setSetPoint(t*2000);
+            }
+            rightPid->setProcessValue(fabs(rightVelocity));
+            rightMotor->speed(rightDirection * t);
+        } else {
+            rightPulsesGoTo=0;
+            this->StopRight();
+        }
+    } else {
+        rightPulsesGoTo=0;
+        this->StopRight();
+
+    }
+    if ((leftPulsesGoTo!=0)&&(leftSpeed>0)) {
+        if (leftPulses < leftPulsesGoTo) {
+            leftPulses =  (int)leftDirection * leftQei->getPulses();
+            leftVelocity = (leftPulses - leftPrevPulses) / RATE;
+            leftPrevPulses = leftPulses;
+            t=leftPid->compute();            
+            if(((leftPulsesGoTo-leftPulses)/leftVelocity)<1.0){               
+                leftPid->reset();
+                leftPid->setSetPoint(t*2000);
+            }
+            leftPid->setProcessValue(fabs(leftVelocity));            
+            leftMotor->speed(leftDirection * t);
+        } else {
+            leftPulsesGoTo=0;
+            this->StopLeft();
+        }
+    } else {
+        leftPulsesGoTo=0;
+        this->StopLeft();
+    }
+}