Robot.cpp

00001 #include "Robot.h"
00002 #include "mbed.h"
00003 
00004 // This library was designed simplify operation of Stinger robot only with MBED and two H-Bridges.
00005 // Is is still in development.
00006 // Class Robot integrated QEI, PID and Motor classes to work together through use of Ticker function to 
00007 // regularly poll QEI objects, adjust PID and Motor objects.
00008 // Right now all pins have been preassigned, later empty constructor will be added with individual pin assignments.
00009 //    Pinouts are:
00010 //    rightMotor = new Motor(p23, p6, p5); // pwm, fwd, rev
00011 //    leftMotor = new Motor(p22, p8, p7); // pwm, fwd, rev
00012 //    leftQei = new QEI(p29,  p30, NC, 624, QEI::X2_ENCODING);  //chanA, chanB, index, ppr
00013 //    rightQei = new QEI(p27, p28, NC, 624, QEI::X2_ENCODING);  //chanB, chanA, index, ppr
00014 //    leftPid = new PID(0.4312, 0.1, 0.0, RATE);  //Kc, Ti, Td
00015 //    rightPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td
00016 
00017 Robot::Robot() {
00018     rightMotor = new Motor(p23, p6, p5); // pwm, fwd, rev
00019     leftMotor = new Motor(p22, p8, p7); // pwm, fwd, rev
00020     leftQei = new QEI(p29,  p30, NC, 624, QEI::X2_ENCODING);  //chanA, chanB, index, ppr
00021     rightQei = new QEI(p27, p28, NC, 624, QEI::X2_ENCODING);  //chanB, chanA, index, ppr
00022     leftPid = new PID(0.4312, 0.1, 0.0, RATE);  //Kc, Ti, Td
00023     rightPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td
00024     leftMotor->period(0.00005);  //Set motor PWM periods to 20KHz.
00025     rightMotor->period(0.00005);
00026     leftPid->setInputLimits(0, MAX_SPEED);
00027     leftPid->setOutputLimits(0.3, 1);
00028     leftPid->setMode(AUTO_MODE);
00029     rightPid->setInputLimits(0, MAX_SPEED);
00030     rightPid->setOutputLimits(0.3, 1);
00031     rightPid->setMode(AUTO_MODE);
00032     leftPulsesGoTo=rightPulsesGoTo=0;
00033     ticker.attach(this, &Robot::Call, RATE); // the address of the function to be attached (call) and the interval (RATE)
00034 }
00035 
00036 Robot::~Robot() {
00037     delete rightMotor;
00038     delete leftMotor;
00039     delete leftQei;
00040     delete rightQei;
00041     delete leftMotor;
00042     delete rightMotor;
00043     delete leftPid;
00044     delete rightPid;
00045     ticker.detach();
00046 }
00047 
00048 void Robot::MoveStraightPulses(float s, int pulses) {
00049     leftQei->reset();
00050     rightQei->reset();
00051     leftPulses=rightPulses=0;
00052     if (s>=0.0)leftDirection=rightDirection=1.0;
00053     else leftDirection=rightDirection=-1.0;
00054     leftSpeed=abs(s*MAX_SPEED);
00055     rightSpeed=abs(s*MAX_SPEED);
00056     leftPid->reset();
00057     rightPid->reset();
00058     leftPid->setSetPoint(leftSpeed);
00059     rightPid->setSetPoint(rightSpeed);
00060     leftPulsesGoTo=rightPulsesGoTo=pulses*2;
00061     if (s==0)Stop();
00062 }
00063 
00064 void Robot::MoveStraightInches(float speed, float in){
00065     MoveStraightPulses(speed, (int)(in*85.6144));
00066 }
00067 
00068 void Robot::MoveStraightRotations(float speed, float rotations){
00069     MoveStraightPulses(speed, (int)(rotations*624.00));
00070 }
00071 
00072 void Robot::Stop() {
00073     this->StopLeft();
00074     this->StopRight();
00075 }
00076 
00077 void Robot::StopLeft() {
00078     leftMotor->brake();
00079     leftPulsesGoTo=0;
00080     leftSpeed=0.0;
00081     leftPulses=0;
00082     leftQei->reset();
00083     
00084     leftPid->reset();
00085     leftPid->setSetPoint(0);
00086 }
00087 
00088 void Robot::StopRight() {
00089     rightMotor->brake();
00090     rightPulsesGoTo=0;
00091     rightSpeed=0.0;
00092     rightPulses=0;
00093     rightQei->reset();
00094     rightPid->reset();
00095     rightPid->setSetPoint(0);
00096     
00097 
00098 }
00099 void Robot::RotateLeftWheel(float speed, float deg) {
00100     leftPid->reset();
00101     leftSpeed=abs(speed*MAX_SPEED);
00102     if (speed>=0.0)leftDirection=1.0;
00103     else leftDirection=-1.0;
00104     leftPulsesGoTo=deg*624/360;
00105     leftPid->setSetPoint(leftSpeed);
00106 }
00107 
00108 void Robot::RotateRightWheel(float speed, float deg) {
00109     rightPid->reset();
00110     rightSpeed=abs(speed*MAX_SPEED);
00111     if (speed>=0.0)rightDirection=1.0;
00112     else leftDirection=-1.0;
00113     rightPulsesGoTo=deg*624/360;
00114     rightPid->setSetPoint(rightSpeed);
00115 }
00116 
00117 void Robot::PivetLeft(float deg) {
00118     if (deg<0.0) {
00119         PivetRight(-deg);
00120         return;
00121     }
00122     leftPid->reset();
00123     rightPid->reset();
00124     leftSpeed=abs(0.5*MAX_SPEED);
00125     rightSpeed=abs(0.5*MAX_SPEED);
00126     leftDirection=-1.0;
00127     rightDirection=1.0;
00128     leftPulsesGoTo=abs(deg*DPP);
00129     rightPulsesGoTo=abs(deg*DPP);
00130     leftPid->setSetPoint(leftSpeed);
00131     rightPid->setSetPoint(rightSpeed);
00132 }
00133 
00134 void Robot::PivetRight(float deg) {
00135     if (deg<0) {
00136         PivetLeft(-deg);
00137         return;
00138     }
00139     leftPid->reset();
00140     rightPid->reset();
00141     leftSpeed=abs(0.5*MAX_SPEED);
00142     rightSpeed=abs(0.5*MAX_SPEED);
00143     leftDirection=1.0;
00144     rightDirection=-1.0;
00145     leftPulsesGoTo=abs(deg*DPP);
00146     rightPulsesGoTo=abs(deg*DPP);
00147     leftPid->setSetPoint(leftSpeed);
00148     rightPid->setSetPoint(rightSpeed);
00149 }
00150 
00151 int  Robot::IsBusy() {
00152     if ((rightSpeed==0.0)&&(leftSpeed==0.0))return 0;
00153     return 1;
00154 }
00155 
00156 void Robot::Call() {
00157     float t;
00158     if ((rightPulsesGoTo!=0)&&(rightSpeed>0)) {
00159         if (rightPulses < rightPulsesGoTo) {
00160             rightPulses = (int)rightDirection * rightQei->getPulses();
00161             rightVelocity = (rightPulses - rightPrevPulses) / RATE;
00162             rightPrevPulses = rightPulses;
00163             t=rightPid->compute();
00164             if(((rightPulsesGoTo-rightPulses)/rightVelocity)<1.0){              
00165                 rightPid->reset();
00166                 rightPid->setSetPoint(t*2000);
00167             }
00168             rightPid->setProcessValue(fabs(rightVelocity));
00169             rightMotor->speed(rightDirection * t);
00170         } else {
00171             rightPulsesGoTo=0;
00172             this->StopRight();
00173         }
00174     } else {
00175         rightPulsesGoTo=0;
00176         this->StopRight();
00177 
00178     }
00179     if ((leftPulsesGoTo!=0)&&(leftSpeed>0)) {
00180         if (leftPulses < leftPulsesGoTo) {
00181             leftPulses =  (int)leftDirection * leftQei->getPulses();
00182             leftVelocity = (leftPulses - leftPrevPulses) / RATE;
00183             leftPrevPulses = leftPulses;
00184             t=leftPid->compute();            
00185             if(((leftPulsesGoTo-leftPulses)/leftVelocity)<1.0){               
00186                 leftPid->reset();
00187                 leftPid->setSetPoint(t*2000);
00188             }
00189             leftPid->setProcessValue(fabs(leftVelocity));            
00190             leftMotor->speed(leftDirection * t);
00191         } else {
00192             leftPulsesGoTo=0;
00193             this->StopLeft();
00194         }
00195     } else {
00196         leftPulsesGoTo=0;
00197         this->StopLeft();
00198     }
00199 }