biorobotics group 19 , 2 november
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_controlv2 by
Controller.cpp@4:bfdcf3da7cff, 2016-10-28 (annotated)
- Committer:
- fabian101
- Date:
- Fri Oct 28 12:49:02 2016 +0000
- Revision:
- 4:bfdcf3da7cff
- Parent:
- 2:6b913f93bc0b
- Child:
- 6:a4440eec3652
28-10, filtering needs tweaking, controller needs testing, some fix-me's need to be done
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fabian101 | 0:d935ea6f5c25 | 1 | #include "Controller.h" |
fabian101 | 1:b9005f2aabf5 | 2 | |
fabian101 | 0:d935ea6f5c25 | 3 | int main() { |
fabian101 | 4:bfdcf3da7cff | 4 | //pc.baud(115200); |
fabian101 | 4:bfdcf3da7cff | 5 | //pc.printf("startup: \n\r"); |
fabian101 | 4:bfdcf3da7cff | 6 | //calibrateButton.rise(&calibrate); |
fabian101 | 4:bfdcf3da7cff | 7 | |
fabian101 | 1:b9005f2aabf5 | 8 | initMotors(); |
fabian101 | 1:b9005f2aabf5 | 9 | initControllers(); |
fabian101 | 4:bfdcf3da7cff | 10 | //calibratePower(); // store max amplitude each emg signal |
fabian101 | 4:bfdcf3da7cff | 11 | while(true) |
fabian101 | 4:bfdcf3da7cff | 12 | { |
fabian101 | 4:bfdcf3da7cff | 13 | //calibrate(); // calibrate position |
fabian101 | 4:bfdcf3da7cff | 14 | 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 |
fabian101 | 4:bfdcf3da7cff | 15 | run(); |
fabian101 | 4:bfdcf3da7cff | 16 | } |
fabian101 | 0:d935ea6f5c25 | 17 | } |
fabian101 | 0:d935ea6f5c25 | 18 | |
fabian101 | 4:bfdcf3da7cff | 19 | void run() |
fabian101 | 4:bfdcf3da7cff | 20 | { |
fabian101 | 4:bfdcf3da7cff | 21 | |
fabian101 | 4:bfdcf3da7cff | 22 | motorControl.attach(&setUpdate_flag, RATE); // rate =0.001 |
fabian101 | 4:bfdcf3da7cff | 23 | while(button) // button not presseed |
fabian101 | 4:bfdcf3da7cff | 24 | { |
fabian101 | 4:bfdcf3da7cff | 25 | //pc.printf("run: %d\n\r", counter); |
fabian101 | 4:bfdcf3da7cff | 26 | |
fabian101 | 4:bfdcf3da7cff | 27 | if(update_flag) |
fabian101 | 4:bfdcf3da7cff | 28 | { |
fabian101 | 4:bfdcf3da7cff | 29 | led = !led; |
fabian101 | 4:bfdcf3da7cff | 30 | //counter++; |
fabian101 | 2:6b913f93bc0b | 31 | // returns filtered value |
fabian101 | 2:6b913f93bc0b | 32 | r += signal.readValue(1)/SAMPLES_PER_AVERAGE; |
fabian101 | 4:bfdcf3da7cff | 33 | float r1= signal.readValue(1); |
fabian101 | 2:6b913f93bc0b | 34 | l += signal.readValue(2)/SAMPLES_PER_AVERAGE; |
fabian101 | 2:6b913f93bc0b | 35 | u += signal.readValue(3)/SAMPLES_PER_AVERAGE; |
fabian101 | 2:6b913f93bc0b | 36 | d += signal.readValue(4)/SAMPLES_PER_AVERAGE; |
fabian101 | 4:bfdcf3da7cff | 37 | |
fabian101 | 4:bfdcf3da7cff | 38 | scope.set(0,r); |
fabian101 | 4:bfdcf3da7cff | 39 | scope.set(1,r1); |
fabian101 | 4:bfdcf3da7cff | 40 | scope.send(); |
fabian101 | 4:bfdcf3da7cff | 41 | i++; |
fabian101 | 4:bfdcf3da7cff | 42 | update_flag = false; |
fabian101 | 4:bfdcf3da7cff | 43 | } |
fabian101 | 4:bfdcf3da7cff | 44 | if(i==SAMPLES_PER_AVERAGE) // the filtering works on a frequency which is 10 times higher, |
fabian101 | 4:bfdcf3da7cff | 45 | { |
fabian101 | 4:bfdcf3da7cff | 46 | //counter_motor++; |
fabian101 | 4:bfdcf3da7cff | 47 | //scope.set(0,counter_motor); // emg1 |
fabian101 | 4:bfdcf3da7cff | 48 | |
fabian101 | 2:6b913f93bc0b | 49 | // calculate r,l,u,d in a % of max |
fabian101 | 2:6b913f93bc0b | 50 | double percentageR = r/rMax; |
fabian101 | 2:6b913f93bc0b | 51 | double percentageL = l/lMax; |
fabian101 | 2:6b913f93bc0b | 52 | double percentageU = u/uMax; |
fabian101 | 2:6b913f93bc0b | 53 | double percentageD = d/dMax; |
fabian101 | 2:6b913f93bc0b | 54 | |
fabian101 | 2:6b913f93bc0b | 55 | double minXThreshold; |
fabian101 | 2:6b913f93bc0b | 56 | double minYThreshold; |
fabian101 | 2:6b913f93bc0b | 57 | |
fabian101 | 2:6b913f93bc0b | 58 | r=0; l=0; u=0; d=0; |
fabian101 | 4:bfdcf3da7cff | 59 | |
fabian101 | 4:bfdcf3da7cff | 60 | // ignore the weaker signal,motorValue calculation, threshold set |
fabian101 | 4:bfdcf3da7cff | 61 | |
fabian101 | 0:d935ea6f5c25 | 62 | |
fabian101 | 4:bfdcf3da7cff | 63 | if ( percentageR > percentageL) |
fabian101 | 4:bfdcf3da7cff | 64 | { // horizontal speed request |
fabian101 | 2:6b913f93bc0b | 65 | motorValueX = percentageR; |
fabian101 | 4:bfdcf3da7cff | 66 | minXThreshold = 0.2f*rMax; // set the threshold for quantizer |
fabian101 | 4:bfdcf3da7cff | 67 | } |
fabian101 | 4:bfdcf3da7cff | 68 | else |
fabian101 | 4:bfdcf3da7cff | 69 | { |
fabian101 | 4:bfdcf3da7cff | 70 | motorValueX = -percentageL; |
fabian101 | 4:bfdcf3da7cff | 71 | minXThreshold = 0.2f*lMax; |
fabian101 | 4:bfdcf3da7cff | 72 | } |
fabian101 | 4:bfdcf3da7cff | 73 | if (percentageU > percentageD) // vertical speed request |
fabian101 | 4:bfdcf3da7cff | 74 | { |
fabian101 | 2:6b913f93bc0b | 75 | motorValueY = percentageU; |
fabian101 | 2:6b913f93bc0b | 76 | minYThreshold = 0.2f*uMax; |
fabian101 | 4:bfdcf3da7cff | 77 | } |
fabian101 | 4:bfdcf3da7cff | 78 | else |
fabian101 | 4:bfdcf3da7cff | 79 | { |
fabian101 | 4:bfdcf3da7cff | 80 | motorValueY = -percentageD; |
fabian101 | 4:bfdcf3da7cff | 81 | minYThreshold = 0.2f*dMax; |
fabian101 | 4:bfdcf3da7cff | 82 | } |
fabian101 | 2:6b913f93bc0b | 83 | // current pulses |
fabian101 | 4:bfdcf3da7cff | 84 | xPulses = X_Motor.getPulses(); |
fabian101 | 4:bfdcf3da7cff | 85 | yPulses = Y_Motor.getPulses(); |
fabian101 | 0:d935ea6f5c25 | 86 | |
fabian101 | 1:b9005f2aabf5 | 87 | //edge detection |
fabian101 | 4:bfdcf3da7cff | 88 | if (xPulses > (x_max-margin) && motorValueX > 0) |
fabian101 | 4:bfdcf3da7cff | 89 | { // right should correspond to positive motorvalue |
fabian101 | 2:6b913f93bc0b | 90 | motorValueX = 0; |
fabian101 | 1:b9005f2aabf5 | 91 | } |
fabian101 | 4:bfdcf3da7cff | 92 | if (xPulses < margin && motorValueX < 0) |
fabian101 | 4:bfdcf3da7cff | 93 | { |
fabian101 | 2:6b913f93bc0b | 94 | motorValueX = 0; |
fabian101 | 0:d935ea6f5c25 | 95 | } |
fabian101 | 4:bfdcf3da7cff | 96 | if (yPulses > (y_max-margin) && motorValueY > 0) |
fabian101 | 4:bfdcf3da7cff | 97 | { // up should correspond to positive motorvalue |
fabian101 | 2:6b913f93bc0b | 98 | motorValueY = 0; |
fabian101 | 0:d935ea6f5c25 | 99 | } |
fabian101 | 4:bfdcf3da7cff | 100 | if (yPulses < margin && motorValueY < 0) |
fabian101 | 4:bfdcf3da7cff | 101 | { |
fabian101 | 2:6b913f93bc0b | 102 | motorValueY =0; |
fabian101 | 1:b9005f2aabf5 | 103 | } |
fabian101 | 0:d935ea6f5c25 | 104 | |
fabian101 | 1:b9005f2aabf5 | 105 | // Quantize the linear speed request |
fabian101 | 1:b9005f2aabf5 | 106 | // motorValue is a value [-1,1], currently only 0 or 1/-1 |
fabian101 | 2:6b913f93bc0b | 107 | if (motorValueX > maxXValue){motorValueX = maxXValue;} // safety |
fabian101 | 2:6b913f93bc0b | 108 | if (motorValueX < -maxXValue){motorValueX = -maxXValue;} |
fabian101 | 2:6b913f93bc0b | 109 | if (motorValueX > minXThreshold && motorValueX < maxXValue){motorValueX = maxXValue;} // if threshold is met pwm is 100%, maybe quantize here |
fabian101 | 2:6b913f93bc0b | 110 | if (motorValueX < -minXThreshold && motorValueX > -maxXValue){motorValueX = -maxXValue;} |
fabian101 | 0:d935ea6f5c25 | 111 | |
fabian101 | 2:6b913f93bc0b | 112 | if (motorValueY > maxYValue){motorValueY = maxYValue;} |
fabian101 | 2:6b913f93bc0b | 113 | if (motorValueY < -maxYValue){motorValueY = -maxYValue;} |
fabian101 | 2:6b913f93bc0b | 114 | if (motorValueY > minYThreshold && motorValueY < maxYValue){motorValueY = maxYValue;} |
fabian101 | 2:6b913f93bc0b | 115 | if (motorValueY < -minYThreshold && motorValueY > -maxYValue){motorValueY = -maxYValue;} |
fabian101 | 2:6b913f93bc0b | 116 | |
fabian101 | 2:6b913f93bc0b | 117 | // current position of pen in Meters |
fabian101 | 4:bfdcf3da7cff | 118 | float currentX = (X_Motor.getPulses()/(8400.0f))*(2*PI)*RmX; |
fabian101 | 4:bfdcf3da7cff | 119 | float currentY = (X_Motor.getPulses()/(8400.0f))*(2*PI)*Rpulley; |
fabian101 | 1:b9005f2aabf5 | 120 | |
fabian101 | 2:6b913f93bc0b | 121 | // upcoming position of pen in Meters |
fabian101 | 2:6b913f93bc0b | 122 | //toX = currentX + motorValueX*maxSpeed*RATE; // speed is in m/s rate is in s |
fabian101 | 2:6b913f93bc0b | 123 | //toY = currentY + motorValueY*maxSpeed*RATE; |
fabian101 | 2:6b913f93bc0b | 124 | |
fabian101 | 2:6b913f93bc0b | 125 | // distance it should travel for 1 interval in Meters |
fabian101 | 2:6b913f93bc0b | 126 | float deltaX = motorValueX*maxSpeed*RATE; |
fabian101 | 2:6b913f93bc0b | 127 | float deltaY = motorValueY*maxSpeed*RATE; |
fabian101 | 1:b9005f2aabf5 | 128 | |
fabian101 | 2:6b913f93bc0b | 129 | // desired velocity in m/s for this interval |
fabian101 | 2:6b913f93bc0b | 130 | float toVelX = deltaX/RATE; |
fabian101 | 2:6b913f93bc0b | 131 | float toVelY = deltaY/RATE; |
fabian101 | 2:6b913f93bc0b | 132 | |
fabian101 | 2:6b913f93bc0b | 133 | // set setpoint of PID controller, this is the thing you want |
fabian101 | 2:6b913f93bc0b | 134 | X_Controller.setSetPoint(toVelX); |
fabian101 | 2:6b913f93bc0b | 135 | Y_Controller.setSetPoint(toVelY); |
fabian101 | 0:d935ea6f5c25 | 136 | |
fabian101 | 2:6b913f93bc0b | 137 | // current velocity in M/s, the thing you measure |
fabian101 | 2:6b913f93bc0b | 138 | // horizontal motor |
fabian101 | 2:6b913f93bc0b | 139 | float currVelX = (currentX - prevX)/RATE; |
fabian101 | 2:6b913f93bc0b | 140 | prevX = currentX; |
fabian101 | 2:6b913f93bc0b | 141 | X_Controller.setProcessValue(currVelX); |
fabian101 | 1:b9005f2aabf5 | 142 | |
fabian101 | 2:6b913f93bc0b | 143 | //vertical motor |
fabian101 | 2:6b913f93bc0b | 144 | float currVelY = (currentY - prevY)/RATE; |
fabian101 | 2:6b913f93bc0b | 145 | prevY = currentY; |
fabian101 | 2:6b913f93bc0b | 146 | Y_Controller.setProcessValue(currVelY); |
fabian101 | 1:b9005f2aabf5 | 147 | |
fabian101 | 4:bfdcf3da7cff | 148 | // compute the output |
fabian101 | 4:bfdcf3da7cff | 149 | float X_MotorOutput = X_Controller.compute(); |
fabian101 | 4:bfdcf3da7cff | 150 | if (X_MotorOutput > 0) |
fabian101 | 4:bfdcf3da7cff | 151 | { // right request |
fabian101 | 4:bfdcf3da7cff | 152 | X_Direction.write(CW); |
fabian101 | 4:bfdcf3da7cff | 153 | X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1 |
fabian101 | 4:bfdcf3da7cff | 154 | } |
fabian101 | 4:bfdcf3da7cff | 155 | else |
fabian101 | 4:bfdcf3da7cff | 156 | { |
fabian101 | 4:bfdcf3da7cff | 157 | X_Direction.write(!CW); // left request |
fabian101 | 4:bfdcf3da7cff | 158 | X_Magnitude.write(-X_MotorOutput); //motor output will be negative and you need to write a pos value |
fabian101 | 2:6b913f93bc0b | 159 | } |
fabian101 | 4:bfdcf3da7cff | 160 | |
fabian101 | 4:bfdcf3da7cff | 161 | float Y_MotorOutput = Y_Controller.compute(); |
fabian101 | 4:bfdcf3da7cff | 162 | if (Y_MotorOutput > 0) |
fabian101 | 4:bfdcf3da7cff | 163 | { // up request |
fabian101 | 4:bfdcf3da7cff | 164 | Y_Direction.write(CW); |
fabian101 | 4:bfdcf3da7cff | 165 | Y_Magnitude.write(Y_MotorOutput); |
fabian101 | 4:bfdcf3da7cff | 166 | } |
fabian101 | 4:bfdcf3da7cff | 167 | else |
fabian101 | 4:bfdcf3da7cff | 168 | { |
fabian101 | 4:bfdcf3da7cff | 169 | Y_Direction.write(!CW); // down request |
fabian101 | 4:bfdcf3da7cff | 170 | Y_Magnitude.write(-Y_MotorOutput); |
fabian101 | 4:bfdcf3da7cff | 171 | } |
fabian101 | 4:bfdcf3da7cff | 172 | i = 0; |
fabian101 | 0:d935ea6f5c25 | 173 | } |
fabian101 | 0:d935ea6f5c25 | 174 | } |
fabian101 | 0:d935ea6f5c25 | 175 | } |
fabian101 | 0:d935ea6f5c25 | 176 | |
fabian101 | 4:bfdcf3da7cff | 177 | void setUpdate_flag() { |
fabian101 | 4:bfdcf3da7cff | 178 | update_flag = true; |
fabian101 | 1:b9005f2aabf5 | 179 | } |
fabian101 | 1:b9005f2aabf5 | 180 | |
fabian101 | 1:b9005f2aabf5 | 181 | void calibrate() { |
fabian101 | 2:6b913f93bc0b | 182 | motorControl.detach(); |
fabian101 | 2:6b913f93bc0b | 183 | // GO-TO pos (0,0) |
fabian101 | 4:bfdcf3da7cff | 184 | while ((X_Motor.getPulses() > margin) && (Y_Motor.getPulses() > margin)){ |
fabian101 | 4:bfdcf3da7cff | 185 | if (X_Motor.getPulses() > margin){ |
fabian101 | 1:b9005f2aabf5 | 186 | // go direction LEFT |
fabian101 | 2:6b913f93bc0b | 187 | X_Direction.write(!CW); |
fabian101 | 4:bfdcf3da7cff | 188 | X_Magnitude.write(1.0); // WRONG, reqVel must be 1cm/s here |
fabian101 | 2:6b913f93bc0b | 189 | } else { //motormagX 0 |
fabian101 | 2:6b913f93bc0b | 190 | X_Direction.write(CW); |
fabian101 | 2:6b913f93bc0b | 191 | X_Magnitude.write(0.0); |
fabian101 | 2:6b913f93bc0b | 192 | } |
fabian101 | 4:bfdcf3da7cff | 193 | if (Y_Motor.getPulses() > margin){ |
fabian101 | 1:b9005f2aabf5 | 194 | // go direction DOWN |
fabian101 | 2:6b913f93bc0b | 195 | Y_Direction.write(!CW); |
fabian101 | 2:6b913f93bc0b | 196 | Y_Magnitude.write(1.0); |
fabian101 | 2:6b913f93bc0b | 197 | } else { //motormagY 0 |
fabian101 | 2:6b913f93bc0b | 198 | Y_Direction.write(CW); |
fabian101 | 2:6b913f93bc0b | 199 | Y_Magnitude.write(0.0); |
fabian101 | 2:6b913f93bc0b | 200 | } |
fabian101 | 1:b9005f2aabf5 | 201 | } |
fabian101 | 4:bfdcf3da7cff | 202 | prevX = (X_Motor.getPulses()/(8400.0f))*(2*PI)*RmX; |
fabian101 | 4:bfdcf3da7cff | 203 | prevY = (X_Motor.getPulses()/(8400.0f))*(2*PI)*Rpulley; |
fabian101 | 2:6b913f93bc0b | 204 | r=0; l=0; u=0; d=0; |
fabian101 | 4:bfdcf3da7cff | 205 | i=0; |
fabian101 | 1:b9005f2aabf5 | 206 | } |
fabian101 | 1:b9005f2aabf5 | 207 | |
fabian101 | 1:b9005f2aabf5 | 208 | void calibratePower(){ |
fabian101 | 4:bfdcf3da7cff | 209 | while((clock()/CLOCKS_PER_SEC) < 5) |
fabian101 | 4:bfdcf3da7cff | 210 | { // FIXME |
fabian101 | 1:b9005f2aabf5 | 211 | // do this for 5 sec |
fabian101 | 4:bfdcf3da7cff | 212 | float rCal = signal.readValue(1); |
fabian101 | 4:bfdcf3da7cff | 213 | if ( rCal > rMax) |
fabian101 | 4:bfdcf3da7cff | 214 | { |
fabian101 | 1:b9005f2aabf5 | 215 | rMax = rCal; |
fabian101 | 1:b9005f2aabf5 | 216 | } |
fabian101 | 4:bfdcf3da7cff | 217 | float lCal = signal.readValue(2); |
fabian101 | 4:bfdcf3da7cff | 218 | if ( lCal > lMax) |
fabian101 | 4:bfdcf3da7cff | 219 | { |
fabian101 | 1:b9005f2aabf5 | 220 | lMax = lCal; |
fabian101 | 1:b9005f2aabf5 | 221 | } |
fabian101 | 4:bfdcf3da7cff | 222 | float uCal = signal.readValue(3); |
fabian101 | 4:bfdcf3da7cff | 223 | if ( uCal > uMax) |
fabian101 | 4:bfdcf3da7cff | 224 | { |
fabian101 | 1:b9005f2aabf5 | 225 | uMax = uCal; |
fabian101 | 1:b9005f2aabf5 | 226 | } |
fabian101 | 4:bfdcf3da7cff | 227 | float dCal = signal.readValue(4); |
fabian101 | 4:bfdcf3da7cff | 228 | if ( dCal > dMax) |
fabian101 | 4:bfdcf3da7cff | 229 | { |
fabian101 | 1:b9005f2aabf5 | 230 | dMax = dCal; |
fabian101 | 1:b9005f2aabf5 | 231 | } |
fabian101 | 1:b9005f2aabf5 | 232 | } |
fabian101 | 1:b9005f2aabf5 | 233 | return; |
fabian101 | 1:b9005f2aabf5 | 234 | } |