biorobotics group 19 , 2 november
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_controlv2 by
Diff: Controller.cpp
- Revision:
- 1:b9005f2aabf5
- Parent:
- 0:d935ea6f5c25
- Child:
- 2:6b913f93bc0b
diff -r d935ea6f5c25 -r b9005f2aabf5 Controller.cpp --- a/Controller.cpp Fri Oct 21 09:13:53 2016 +0000 +++ b/Controller.cpp Fri Oct 21 21:32:20 2016 +0000 @@ -1,9 +1,12 @@ #include "initialize.h" #include "Controller.h" + int main() { calibrateButton.rise(&calibrate); motorControl.attach(&setFlag, RATE); - //[FIXME]calibratePowerrrr(); store max amplitude each emg signal + initMotors(); + initControllers(); + calibratePower(); // store max amplitude each emg signal run(); } @@ -21,51 +24,81 @@ double percentageD = d/dMax; - if (flag) { - // ignore the weaker signal and motorValue is calculate here - if ( percentageR > percentageL){ // horizontal speed + if (flag) { // 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 + if ( percentageR > percentageL){ // horizontal speed request motorValueHor = percentageR; - } else {motorValueHor = -percentageL;} - if (percentageU > percentageD) { // vertical speed + minHorThreshold = 0.2*rMax; // set the threshold + } else {motorValueHor = -percentageL; + minHorThreshold = 0.2*lMax; + } + if (percentageU > percentageD) { // vertical speed request motorValueVer = percentageU; - } else {motorValueVer = -percentageD;} - + minVerThreshold = 0.2*uMax; + } else {motorValueVer = -percentageD; + minVerThreshold = 0.2*uMax; + } // current pulses and prevpulses calculated x += (pulses.getDistanceHor() - prevX); // in pulses prevX = x; y += (pulses.getDistanceVer() - prevY); prevY = y; - //edge detection: for now it will calibrate; implement function that allows movement for the other directions - if (x > x_max || x < 0) { - emergencyExit(); + //edge detection + if (x > x_max && motorValueHor > 0) { // right should correspond to positive motorvalue + motorValueHor = 0; + } + if (x < 0 && motorValueHor < 0) { // carefulwith 0 pulses, maybe implement margin + motorValueHor = 0; } - if (y > y_max || y < 0) { // - emergencyExit(); + if (y > y_max && motorValueVer > 0) { // up should correspond to positive motorvalue + motorValueVer = 0; } + if (y < 0 && motorValueVer < 0) { + motorValueVer =0; + } - // Quantize - if (motorValueHor > maxHorVel){motorValueHor = maxHorVel;} - if (motorValueHor < -maxHorVel){motorValueHor = -maxHorVel;} - if (motorValueHor > 0 && motorValueHor < maxHorVel){motorValueHor = 0.5*maxHorVel;} - if (motorValueHor < 0 && motorValueHor > -maxHorVel){motorValueHor = -0.5*maxHorVel;} + // Quantize the linear speed request + // motorValue is a value [-1,1], currently only 0 or 1/-1 + if (motorValueHor > maxHorValue){motorValueHor = maxHorValue;} // safety + if (motorValueHor < -maxHorValue){motorValueHor = -maxHorValue;} + if (motorValueHor > minHorThreshold && motorValueHor < maxHorValue){motorValueHor = maxHorValue;} // if threshold is met pwm is 100%, maybe quantize here + if (motorValueHor < -minHorThreshold && motorValueHor > -maxHorValue){motorValueHor = -maxHorValue;} - if ( motorValueVer > maxVerVel){motorValueVer = 1;} - if (motorValueVer < -1){motorValueVer = -1;} - if (motorValueVer > 0 && motorValueVer < 1){motorValueVer = 0.5;} - if (motorValueVer < 0 && motorValueVer > -1){motorValueVer = -0.5;} + if (motorValueVer > maxVerValue){motorValueVer = maxVerValue;} + if (motorValueVer < -maxVerValue){motorValueVer = -maxVerValue;} + if (motorValueVer > minVerThreshold && motorValueVer < maxVerValue){motorValueVer = maxVerValue;} + if (motorValueVer < -minVerThreshold && motorValueVer > -maxVerValue){motorValueVer = -maxVerValue;} + + //current position of pen in degrees + currentXangle = 360.0f*(pulses.getDistanceHor()/8400); + currentYangle = 360.0f*(pulses.getDistanceVer()/8400); + + // current position of pen in cm + currentX = (360.0f/(2*PI))*(pulses.getDistanceHor()/8400)*(RmHor/R1); // radiants*revolutions*gearratio is x pos in rad IS RAD EQUAL TO CM????????? + currentY= (360.0f/(2*PI))*(pulses.getDistanceVer()/8400); // radiants*revolutions 1:1 ratio?? - // Calculate desired rotational velocity from linear velocity + // desired position of pen in cm + toX = currentX + motorValueHor*maxSpeed*RATE; + toY = currentY + motorValueVer*maxSpeed*RATE; + + // desired angle of pen in degrees + toangleX = ((toX-currentX)*(R1/RmHor))*(360.0f/(2*PI)); + toangleY = (toX-currentX)*(360.0f/(2*PI)); + + //requested velocities in degrees/sec + Xvelrequest = (toangleX - currentXangle) / RATE; + Yvelrequest = (toangleY - currentYangle) / RATE; + + // set setpoint of PID controller here with velrequest, what are the input limits??? // Calculate current rotational velocity - // give the PID controller the setpoint and processvariable + // give the PID controller the processvariable // compute PID // output to motor - // motorOutput(toMotorValue(percentageR, percentageL), true); // [FIXME]- safety edges (if statement) - // motorOutput(toMotorValue(percentageU, percentageD), false); // replace this with all the calculations and PID class stuff // output to hidscope flag = false; // only output to motor with RATE @@ -73,30 +106,6 @@ } } -void calibrate() { - // GO-TO pos (0,0) - // pulses can only be 0 or positive so reverse the direction until 0 pulses is reached - // 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? - return; -} - -void motorOutput(double motorValue, bool horizontal) { // IMPLEMENT DEADZONE? - if (motorValue > 1) { // safety - motorValue = 1; - } - if (motorValue < -1) { - motorValue = -1; - } - - // deadzone here - - if (horizontal) { - motor.outputHor(fabs(motorValue), (motorValue>0) ); - } else { - motor.outputVer(fabs(motorValue), (motorValue>0)); - } -} void setFlag() { flag = true; @@ -105,4 +114,46 @@ void emergencyExit() { calibrate(); run(); - } \ No newline at end of file +} + +void calibrate() { + while ((pulses.getDistanceHor() > margin) && (pulses.getDistanceVer() > margin)){ + if (pulses.getDistanceHor() > margin){ + // go direction LEFT + } + else { //motormaghor 0 + } + if (pulses.getDistanceVer() > margin){ + // go direction DOWN + } + else { //motormagver 0} + } + // GO-TO pos (0,0) + // pulses can only be 0 or positive so reverse the direction until 0 pulses is reached + // 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? + } +} + +void calibratePower(){ + while((clock()/CLOCKS_PER_SEC) < 5){ // FIXME + // do this for 5 sec + double rCal = signal.readValue(1); + if ( rCal > rMax){ + rMax = rCal; + } + double lCal = signal.readValue(2); + if ( lCal > lMax){ + lMax = lCal; + } + double uCal = signal.readValue(3); + if ( uCal > uMax){ + uMax = uCal; + } + double dCal = signal.readValue(4); + if ( dCal > dMax){ + dMax = dCal; + } + } + return; +}