main.cpp

00001 //****************************************************************************/
00002 // Includes
00003 #include "PID.h"
00004 #include "QEI.h"
00005 #include "HIDScope.h"
00006 
00007 #include "inits.h" // all globals, pin and variable initialization 
00008 #include "EMG.h"
00009 
00010 void setGoFlag(){
00011     if (goFlag==true){
00012         //pc.printf("rate too high, error in setGoFlag \n\r");
00013         // flag is already set true: code too slow or frequency too high    
00014     }
00015     goFlag=true;
00016 }
00017 
00018 void systemStart(){
00019     systemOn=true;
00020 }
00021 void systemStop(){
00022     systemOn=false;
00023     leftMotor=0;
00024     rightMotor=0;
00025 }
00026 int main() {
00027     
00028     // initialize (defined in inits.h)
00029     initMotors();
00030     initPIDs();
00031     
00032     motorControlTicker.attach(&setGoFlag, RATE);
00033     T1.attach(&samplego, (float)1/Fs);
00034     
00035     cali_button.rise(&calibrate);
00036     startButton.rise(&systemStart);
00037     stopButton.rise(&systemStop);
00038     
00039     endTimer.start(); //Run for 100 seconds.
00040     while (true){
00041         if (goFlag==true && systemOn==true){
00042             // get 'emg' signal
00043             request_pos=y1;
00044             request_neg=y2;
00045             
00046             // determine if forward or backward signal is stronger and set reference
00047             if (request_pos > request_neg){
00048                 request = request_pos; 
00049             } 
00050             else {
00051                 request = -request_neg;
00052             }
00053             leftController.setSetPoint(request);
00054             rightController.setSetPoint(0);
00055             
00056             // *******************
00057             // Velocity calculation
00058             // left
00059             leftPulses = leftQei.getPulses();
00060             leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE;
00061             leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000
00062             leftPrevPulses = leftPulses;
00063             
00064             // right
00065             rightPulses = rightQei.getPulses();
00066             rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE;
00067             rightVelocity = rightVelocity/30000; // scale to 0 - 1, max velocity = 30000
00068             rightPrevPulses = rightPulses;
00069             
00070             // ***********
00071             // PID control
00072             // left
00073             leftController.setProcessValue(leftVelocity);
00074             leftPwmDuty = leftController.compute();
00075             if (leftPwmDuty < 0){
00076                 leftDirection = 0;
00077                 leftMotor = -leftPwmDuty;
00078             }
00079             else {
00080                 leftDirection = 1;
00081                 leftMotor = leftPwmDuty;
00082             }
00083             
00084             // right
00085             rightController.setProcessValue(rightVelocity);
00086             rightPwmDuty = rightController.compute();
00087             if (rightPwmDuty < 0){
00088                 rightDirection = 0;
00089                 rightMotor = -rightPwmDuty;
00090             }
00091             else {
00092                 rightDirection = 1;
00093                 rightMotor = rightPwmDuty;
00094             }
00095             
00096             // User feedback
00097             //scope.set(0, request);
00098             //scope.set(1, leftPwmDuty);
00099             //scope.set(2, leftVelocity);
00100             //scope.send();
00101             pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
00102         
00103             goFlag=false;
00104         }
00105         if(sample_go && systemOn==true)
00106         {
00107             sample_filter();
00108             scope.set(0, y1);
00109             scope.set(1, y2);
00110             scope.send();
00111             sample_go = 0;
00112         }
00113     }
00114 
00115     //Stop motors.
00116     leftMotor  = 0;
00117     rightMotor = 0;
00118     
00119 }