Controller.cpp

00001 #include "initialize.h"
00002 #include "Controller.h"
00003 
00004 int main() {
00005     calibrateButton.rise(&calibrate);
00006     motorControl.attach(&setFlag, RATE); // rate =0.001
00007     initMotors();
00008     initControllers();
00009     calibrate(); // calibrate position
00010     calibratePower(); // store max amplitude each emg signal
00011     run();
00012 }
00013 
00014 void run() {
00015     while(true){
00016         if(flag){
00017             // returns filtered value
00018             r += signal.readValue(1)/SAMPLES_PER_AVERAGE;
00019             l += signal.readValue(2)/SAMPLES_PER_AVERAGE;
00020             u += signal.readValue(3)/SAMPLES_PER_AVERAGE;
00021             d += signal.readValue(4)/SAMPLES_PER_AVERAGE;
00022             }
00023             if(i=10){ // the filtering works on a frequency which is 10 times higher
00024                 
00025             // calculate r,l,u,d in a % of max 
00026             double percentageR = r/rMax;
00027             double percentageL = l/lMax;
00028             double percentageU = u/uMax;
00029             double percentageD = d/dMax;
00030             
00031             double minXThreshold;
00032             double minYThreshold;
00033             
00034             r=0; l=0; u=0; d=0;
00035                 // setting the flag here will cause the edge detection to be ignored if the flag is false for some reason
00036                 // ignore the weaker signal,motorValue calculation, threshold set
00037         } 
00038         
00039         if ( percentageR > percentageL){ // horizontal speed request
00040                 motorValueX = percentageR;
00041                 minXThreshold = 0.2f*rMax; // set the threshold
00042             }   else {motorValueX = -percentageL;
00043                     minXThreshold = 0.2f*lMax;
00044                 }
00045             if (percentageU > percentageD) { // vertical speed request
00046                 motorValueY = percentageU;
00047                 minYThreshold = 0.2f*uMax;
00048             }   else {motorValueY = -percentageD;
00049                     minYThreshold = 0.2f*dMax;
00050                 }
00051             // current pulses
00052             xPulses = pulses.getDistanceX(); // in pulses
00053             yPulses = pulses.getDistanceY();
00054             
00055             //edge detection
00056             if (xPulses > (x_max-margin) && motorValueX > 0) { // right should correspond to positive motorvalue
00057                 motorValueX = 0;
00058             }
00059             if (xPulses < margin && motorValueX < 0) { 
00060                 motorValueX = 0;
00061             }
00062             if (yPulses > (y_max-margin) && motorValueY > 0) { // up should correspond to positive motorvalue
00063                 motorValueY = 0;
00064             }
00065             if (yPulses < margin && motorValueY < 0) {
00066                 motorValueY =0;
00067             } 
00068             
00069             // Quantize the linear speed request
00070             // motorValue is a value [-1,1], currently only 0 or 1/-1
00071             if (motorValueX > maxXValue){motorValueX = maxXValue;} // safety
00072             if (motorValueX < -maxXValue){motorValueX = -maxXValue;}
00073             if (motorValueX > minXThreshold && motorValueX < maxXValue){motorValueX = maxXValue;} // if threshold is met pwm is 100%, maybe quantize here
00074             if (motorValueX < -minXThreshold && motorValueX > -maxXValue){motorValueX = -maxXValue;}
00075                 
00076             if (motorValueY > maxYValue){motorValueY = maxYValue;}
00077             if (motorValueY < -maxYValue){motorValueY = -maxYValue;}
00078             if (motorValueY > minYThreshold && motorValueY < maxYValue){motorValueY = maxYValue;}
00079             if (motorValueY < -minYThreshold && motorValueY > -maxYValue){motorValueY = -maxYValue;}
00080             
00081 
00082             // current position of pen in Meters
00083             float currentX = (pulses.getDistanceX()/(8400.0f))*(2*PI)*RmX;
00084             float currentY = (pulses.getDistanceY()/(8400.0f))*(2*PI)*Rpulley; 
00085             
00086             // upcoming position of pen in Meters
00087             //toX = currentX + motorValueX*maxSpeed*RATE; // speed is in m/s rate is in s
00088             //toY = currentY + motorValueY*maxSpeed*RATE; 
00089             
00090             // distance it should travel for 1 interval in Meters 
00091             float deltaX = motorValueX*maxSpeed*RATE;
00092             float deltaY = motorValueY*maxSpeed*RATE;
00093             
00094             // desired velocity in m/s for this interval
00095             float toVelX = deltaX/RATE; 
00096             float toVelY = deltaY/RATE;
00097             
00098             // set setpoint of PID controller, this is the thing you want
00099             X_Controller.setSetPoint(toVelX);
00100             Y_Controller.setSetPoint(toVelY);
00101             
00102             // current velocity in M/s, the thing you measure
00103             // horizontal motor
00104             float currVelX = (currentX - prevX)/RATE;
00105             prevX = currentX;
00106             X_Controller.setProcessValue(currVelX);
00107             
00108             //vertical motor
00109             float currVelY = (currentY - prevY)/RATE;
00110             prevY = currentY;
00111             Y_Controller.setProcessValue(currVelY);
00112             
00113                 // compute the output FIXME[calibratebutton might break the program]
00114                 float X_MotorOutput = X_Controller.compute();
00115                 if (X_MotorOutput > 0){ // right request
00116                     X_Direction.write(CW);
00117                     X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1
00118                 }   else {
00119                         X_Direction.write(!CW); // left request
00120                         X_Magnitude.write(-X_MotorOutput);
00121                     }
00122                         
00123                 float Y_MotorOutput = Y_Controller.compute();
00124                 if (Y_MotorOutput > 0){ // up request
00125                     Y_Direction.write(CW);
00126                     Y_Magnitude.write(Y_MotorOutput);
00127                 }   else {
00128                         Y_Direction.write(!CW); // down request
00129                         Y_Magnitude.write(-Y_MotorOutput);
00130                     }
00131             }
00132            
00133            // output to hidscope
00134            
00135             flag = false; // only output to motor with RATE
00136             i = 1;
00137         }
00138         
00139     }
00140 }
00141 
00142 void setFlag() {
00143     flag = true;
00144     i++;
00145 }
00146 
00147 void calibrate() {
00148     motorControl.detach();
00149     calibrateFlag = false;
00150     // GO-TO pos (0,0)
00151     while ((pulses.getDistanceX() > margin) && (pulses.getDistanceY() > margin)){
00152         if (pulses.getDistanceX() > margin){
00153         // go direction LEFT
00154         X_Direction.write(!CW);
00155         X_Magnitude.write(1.0);
00156         }   else { //motormagX 0 
00157             X_Direction.write(CW);
00158             X_Magnitude.write(0.0);   
00159             }
00160         if (pulses.getDistanceY() > margin){
00161         // go direction DOWN
00162         Y_Direction.write(!CW);
00163         Y_Magnitude.write(1.0);
00164         }   else { //motormagY 0  
00165             Y_Direction.write(CW);
00166             Y_Magnitude.write(0.0);   
00167             }
00168         // problem with the interrupt button, after calibration the program should re-start the run function instead of going further at the point it left
00169         // this will cause 1 iteration to be absolutely wrong -> consequences?
00170 
00171     }
00172     prevX = getDistanceX();
00173     prevY = getDistanceY();
00174     r=0; l=0; u=0; d=0;
00175     i=0;
00176     motorControl.attach(&setFlag, RATE); 
00177 }   
00178  
00179 void calibratePower(){
00180     while((clock()/CLOCKS_PER_SEC) < 5){ // FIXME
00181         // do this for 5 sec
00182         double rCal = signal.readValue(1); 
00183         if ( rCal > rMax){
00184             rMax = rCal;
00185         }
00186         double lCal = signal.readValue(2);
00187         if ( lCal > lMax){
00188             lMax = lCal;
00189         }
00190         double uCal = signal.readValue(3);
00191         if ( uCal > uMax){
00192             uMax = uCal;
00193         }
00194         double dCal = signal.readValue(4);
00195         if ( dCal > dMax){
00196             dMax = dCal;
00197         }
00198     }
00199     return;
00200 }