Group19-Biorobotics
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_control_v3 by
Diff: Controller.cpp
- Revision:
- 4:bfdcf3da7cff
- Parent:
- 2:6b913f93bc0b
- Child:
- 6:a4440eec3652
diff -r 0039223685b8 -r bfdcf3da7cff Controller.cpp --- a/Controller.cpp Wed Oct 26 12:40:26 2016 +0000 +++ b/Controller.cpp Fri Oct 28 12:49:02 2016 +0000 @@ -1,27 +1,51 @@ -#include "initialize.h" #include "Controller.h" int main() { - calibrateButton.rise(&calibrate); - motorControl.attach(&setFlag, RATE); // rate =0.001 + //pc.baud(115200); + //pc.printf("startup: \n\r"); + //calibrateButton.rise(&calibrate); + initMotors(); initControllers(); - calibrate(); // calibrate position - calibratePower(); // store max amplitude each emg signal - run(); + //calibratePower(); // store max amplitude each emg signal + while(true) + { + //calibrate(); // calibrate position + while(!button){} // button not pressed is true, this is here to make sure that the it will only go run if you have let go of the button and calibrated + run(); + } } -void run() { - while(true){ - if(flag){ +void run() +{ + + motorControl.attach(&setUpdate_flag, RATE); // rate =0.001 + while(button) // button not presseed + { + //pc.printf("run: %d\n\r", counter); + + if(update_flag) + { + led = !led; + //counter++; // returns filtered value r += signal.readValue(1)/SAMPLES_PER_AVERAGE; + float r1= signal.readValue(1); l += signal.readValue(2)/SAMPLES_PER_AVERAGE; u += signal.readValue(3)/SAMPLES_PER_AVERAGE; d += signal.readValue(4)/SAMPLES_PER_AVERAGE; - } - if(i=10){ // the filtering works on a frequency which is 10 times higher - + + scope.set(0,r); + scope.set(1,r1); + scope.send(); + i++; + update_flag = false; + } + if(i==SAMPLES_PER_AVERAGE) // the filtering works on a frequency which is 10 times higher, + { + //counter_motor++; + //scope.set(0,counter_motor); // emg1 + // calculate r,l,u,d in a % of max double percentageR = r/rMax; double percentageL = l/lMax; @@ -32,37 +56,49 @@ double minYThreshold; r=0; l=0; u=0; d=0; - // setting the flag here will cause the edge detection to be ignored if the flag is false for some reason - // ignore the weaker signal,motorValue calculation, threshold set - } + + // ignore the weaker signal,motorValue calculation, threshold set + - if ( percentageR > percentageL){ // horizontal speed request + if ( percentageR > percentageL) + { // horizontal speed request motorValueX = percentageR; - minXThreshold = 0.2f*rMax; // set the threshold - } else {motorValueX = -percentageL; - minXThreshold = 0.2f*lMax; - } - if (percentageU > percentageD) { // vertical speed request + minXThreshold = 0.2f*rMax; // set the threshold for quantizer + } + else + { + motorValueX = -percentageL; + minXThreshold = 0.2f*lMax; + } + if (percentageU > percentageD) // vertical speed request + { motorValueY = percentageU; minYThreshold = 0.2f*uMax; - } else {motorValueY = -percentageD; - minYThreshold = 0.2f*dMax; - } + } + else + { + motorValueY = -percentageD; + minYThreshold = 0.2f*dMax; + } // current pulses - xPulses = pulses.getDistanceX(); // in pulses - yPulses = pulses.getDistanceY(); + xPulses = X_Motor.getPulses(); + yPulses = Y_Motor.getPulses(); //edge detection - if (xPulses > (x_max-margin) && motorValueX > 0) { // right should correspond to positive motorvalue + if (xPulses > (x_max-margin) && motorValueX > 0) + { // right should correspond to positive motorvalue motorValueX = 0; } - if (xPulses < margin && motorValueX < 0) { + if (xPulses < margin && motorValueX < 0) + { motorValueX = 0; } - if (yPulses > (y_max-margin) && motorValueY > 0) { // up should correspond to positive motorvalue + if (yPulses > (y_max-margin) && motorValueY > 0) + { // up should correspond to positive motorvalue motorValueY = 0; } - if (yPulses < margin && motorValueY < 0) { + if (yPulses < margin && motorValueY < 0) + { motorValueY =0; } @@ -78,10 +114,9 @@ if (motorValueY > minYThreshold && motorValueY < maxYValue){motorValueY = maxYValue;} if (motorValueY < -minYThreshold && motorValueY > -maxYValue){motorValueY = -maxYValue;} - // current position of pen in Meters - float currentX = (pulses.getDistanceX()/(8400.0f))*(2*PI)*RmX; - float currentY = (pulses.getDistanceY()/(8400.0f))*(2*PI)*Rpulley; + float currentX = (X_Motor.getPulses()/(8400.0f))*(2*PI)*RmX; + float currentY = (X_Motor.getPulses()/(8400.0f))*(2*PI)*Rpulley; // upcoming position of pen in Meters //toX = currentX + motorValueX*maxSpeed*RATE; // speed is in m/s rate is in s @@ -110,54 +145,52 @@ prevY = currentY; Y_Controller.setProcessValue(currVelY); - // compute the output FIXME[calibratebutton might break the program] - float X_MotorOutput = X_Controller.compute(); - if (X_MotorOutput > 0){ // right request - X_Direction.write(CW); - X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1 - } else { - X_Direction.write(!CW); // left request - X_Magnitude.write(-X_MotorOutput); - } - - float Y_MotorOutput = Y_Controller.compute(); - if (Y_MotorOutput > 0){ // up request - Y_Direction.write(CW); - Y_Magnitude.write(Y_MotorOutput); - } else { - Y_Direction.write(!CW); // down request - Y_Magnitude.write(-Y_MotorOutput); - } + // compute the output + float X_MotorOutput = X_Controller.compute(); + if (X_MotorOutput > 0) + { // right request + X_Direction.write(CW); + X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1 + } + else + { + X_Direction.write(!CW); // left request + X_Magnitude.write(-X_MotorOutput); //motor output will be negative and you need to write a pos value } - - // output to hidscope - - flag = false; // only output to motor with RATE - i = 1; + + float Y_MotorOutput = Y_Controller.compute(); + if (Y_MotorOutput > 0) + { // up request + Y_Direction.write(CW); + Y_Magnitude.write(Y_MotorOutput); + } + else + { + Y_Direction.write(!CW); // down request + Y_Magnitude.write(-Y_MotorOutput); + } + i = 0; } - } } -void setFlag() { - flag = true; - i++; +void setUpdate_flag() { + update_flag = true; } void calibrate() { motorControl.detach(); - calibrateFlag = false; // GO-TO pos (0,0) - while ((pulses.getDistanceX() > margin) && (pulses.getDistanceY() > margin)){ - if (pulses.getDistanceX() > margin){ + while ((X_Motor.getPulses() > margin) && (Y_Motor.getPulses() > margin)){ + if (X_Motor.getPulses() > margin){ // go direction LEFT X_Direction.write(!CW); - X_Magnitude.write(1.0); + X_Magnitude.write(1.0); // WRONG, reqVel must be 1cm/s here } else { //motormagX 0 X_Direction.write(CW); X_Magnitude.write(0.0); } - if (pulses.getDistanceY() > margin){ + if (Y_Motor.getPulses() > margin){ // go direction DOWN Y_Direction.write(!CW); Y_Magnitude.write(1.0); @@ -165,34 +198,35 @@ Y_Direction.write(CW); Y_Magnitude.write(0.0); } - // problem with the interrupt button, after calibration the program should re-start the run function instead of going further at the point it left - // this will cause 1 iteration to be absolutely wrong -> consequences? - } - prevX = getDistanceX(); - prevY = getDistanceY(); + prevX = (X_Motor.getPulses()/(8400.0f))*(2*PI)*RmX; + prevY = (X_Motor.getPulses()/(8400.0f))*(2*PI)*Rpulley; r=0; l=0; u=0; d=0; - i=0; - motorControl.attach(&setFlag, RATE); + i=0; } void calibratePower(){ - while((clock()/CLOCKS_PER_SEC) < 5){ // FIXME + while((clock()/CLOCKS_PER_SEC) < 5) + { // FIXME // do this for 5 sec - double rCal = signal.readValue(1); - if ( rCal > rMax){ + float rCal = signal.readValue(1); + if ( rCal > rMax) + { rMax = rCal; } - double lCal = signal.readValue(2); - if ( lCal > lMax){ + float lCal = signal.readValue(2); + if ( lCal > lMax) + { lMax = lCal; } - double uCal = signal.readValue(3); - if ( uCal > uMax){ + float uCal = signal.readValue(3); + if ( uCal > uMax) + { uMax = uCal; } - double dCal = signal.readValue(4); - if ( dCal > dMax){ + float dCal = signal.readValue(4); + if ( dCal > dMax) + { dMax = dCal; } }