biorobotics group 19 , 2 november
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_controlv2 by
Controller.cpp@7:2ab4bdd1f998, 2016-11-02 (annotated)
- Committer:
- fabian101
- Date:
- Wed Nov 02 15:40:15 2016 +0000
- Revision:
- 7:2ab4bdd1f998
- Parent:
- 6:a4440eec3652
biorobotics group 19 , 2 november
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 | 6:a4440eec3652 | 4 | pc.baud(115200); |
fabian101 | 1:b9005f2aabf5 | 5 | initMotors(); |
fabian101 | 1:b9005f2aabf5 | 6 | initControllers(); |
fabian101 | 6:a4440eec3652 | 7 | calibratePower(); // store max amplitude each emg signal |
fabian101 | 6:a4440eec3652 | 8 | |
fabian101 | 4:bfdcf3da7cff | 9 | while(true) |
fabian101 | 4:bfdcf3da7cff | 10 | { |
fabian101 | 6:a4440eec3652 | 11 | calibrate(); // calibrate position |
fabian101 | 4:bfdcf3da7cff | 12 | 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 | 13 | run(); |
fabian101 | 4:bfdcf3da7cff | 14 | } |
fabian101 | 0:d935ea6f5c25 | 15 | } |
fabian101 | 0:d935ea6f5c25 | 16 | |
fabian101 | 4:bfdcf3da7cff | 17 | void run() |
fabian101 | 4:bfdcf3da7cff | 18 | { |
fabian101 | 6:a4440eec3652 | 19 | //pc.printf("ik ga nu runnen G \n\r"); |
fabian101 | 4:bfdcf3da7cff | 20 | motorControl.attach(&setUpdate_flag, RATE); // rate =0.001 |
fabian101 | 4:bfdcf3da7cff | 21 | while(button) // button not presseed |
fabian101 | 4:bfdcf3da7cff | 22 | { |
fabian101 | 6:a4440eec3652 | 23 | if(update_flag) // |
fabian101 | 6:a4440eec3652 | 24 | { |
fabian101 | 2:6b913f93bc0b | 25 | // returns filtered value |
fabian101 | 6:a4440eec3652 | 26 | r = signal.readValue(1); |
fabian101 | 6:a4440eec3652 | 27 | l = signal.readValue(2); |
fabian101 | 6:a4440eec3652 | 28 | //pc.printf("rechts is : %f\n\r", r); |
fabian101 | 6:a4440eec3652 | 29 | //pc.printf("links is : %f\n\r", l); |
fabian101 | 4:bfdcf3da7cff | 30 | |
fabian101 | 6:a4440eec3652 | 31 | //u = signal.readValue(3); |
fabian101 | 6:a4440eec3652 | 32 | // d = signal.readValue(4); |
fabian101 | 6:a4440eec3652 | 33 | // calculate r,l,u,d relative to the max [0,1] |
fabian101 | 6:a4440eec3652 | 34 | percentageR = r/rMax; |
fabian101 | 6:a4440eec3652 | 35 | percentageL = l/lMax; |
fabian101 | 6:a4440eec3652 | 36 | percentageU = u/uMax; |
fabian101 | 6:a4440eec3652 | 37 | percentageD = d/dMax; |
fabian101 | 6:a4440eec3652 | 38 | //scope.set(0,percentageR); |
fabian101 | 6:a4440eec3652 | 39 | //scope.set(1,percentageL); |
fabian101 | 6:a4440eec3652 | 40 | //scope.send(); |
fabian101 | 6:a4440eec3652 | 41 | //pc.printf("wat is percentageL : %f \n\r", percentageL); |
fabian101 | 4:bfdcf3da7cff | 42 | // ignore the weaker signal,motorValue calculation, threshold set |
fabian101 | 4:bfdcf3da7cff | 43 | if ( percentageR > percentageL) |
fabian101 | 4:bfdcf3da7cff | 44 | { // horizontal speed request |
fabian101 | 2:6b913f93bc0b | 45 | motorValueX = percentageR; |
fabian101 | 6:a4440eec3652 | 46 | //pc.printf("rechts > links : %f\n\r", motorValueX); |
fabian101 | 4:bfdcf3da7cff | 47 | } |
fabian101 | 4:bfdcf3da7cff | 48 | else |
fabian101 | 4:bfdcf3da7cff | 49 | { |
fabian101 | 4:bfdcf3da7cff | 50 | motorValueX = -percentageL; |
fabian101 | 6:a4440eec3652 | 51 | //pc.printf("rechts < links : %f\n\r", motorValueX); |
fabian101 | 4:bfdcf3da7cff | 52 | } |
fabian101 | 4:bfdcf3da7cff | 53 | if (percentageU > percentageD) // vertical speed request |
fabian101 | 4:bfdcf3da7cff | 54 | { |
fabian101 | 2:6b913f93bc0b | 55 | motorValueY = percentageU; |
fabian101 | 4:bfdcf3da7cff | 56 | } |
fabian101 | 4:bfdcf3da7cff | 57 | else |
fabian101 | 4:bfdcf3da7cff | 58 | { |
fabian101 | 4:bfdcf3da7cff | 59 | motorValueY = -percentageD; |
fabian101 | 4:bfdcf3da7cff | 60 | } |
fabian101 | 6:a4440eec3652 | 61 | |
fabian101 | 6:a4440eec3652 | 62 | //pc.printf("motorValueX : %f\n\r", motorValueX); |
fabian101 | 2:6b913f93bc0b | 63 | // current pulses |
fabian101 | 4:bfdcf3da7cff | 64 | xPulses = X_Motor.getPulses(); |
fabian101 | 6:a4440eec3652 | 65 | // pc.printf("curentpulses : %f \n\r", xPulses); |
fabian101 | 4:bfdcf3da7cff | 66 | yPulses = Y_Motor.getPulses(); |
fabian101 | 0:d935ea6f5c25 | 67 | |
fabian101 | 1:b9005f2aabf5 | 68 | //edge detection |
fabian101 | 6:a4440eec3652 | 69 | if ((xPulses > (x_max-margin)) && (motorValueX > sensitivityFactor)) // (x_max-margin |
fabian101 | 4:bfdcf3da7cff | 70 | { // right should correspond to positive motorvalue |
fabian101 | 2:6b913f93bc0b | 71 | motorValueX = 0; |
fabian101 | 1:b9005f2aabf5 | 72 | } |
fabian101 | 6:a4440eec3652 | 73 | if ((xPulses < margin) && (motorValueX < (-1 * sensitivityFactor))) |
fabian101 | 4:bfdcf3da7cff | 74 | { |
fabian101 | 2:6b913f93bc0b | 75 | motorValueX = 0; |
fabian101 | 0:d935ea6f5c25 | 76 | } |
fabian101 | 6:a4440eec3652 | 77 | if ((yPulses > (y_max-margin)) && (motorValueY > sensitivityFactor)) |
fabian101 | 4:bfdcf3da7cff | 78 | { // up should correspond to positive motorvalue |
fabian101 | 2:6b913f93bc0b | 79 | motorValueY = 0; |
fabian101 | 0:d935ea6f5c25 | 80 | } |
fabian101 | 6:a4440eec3652 | 81 | if ((yPulses < margin) && (motorValueY < -sensitivityFactor)) |
fabian101 | 4:bfdcf3da7cff | 82 | { |
fabian101 | 2:6b913f93bc0b | 83 | motorValueY =0; |
fabian101 | 1:b9005f2aabf5 | 84 | } |
fabian101 | 1:b9005f2aabf5 | 85 | // Quantize the linear speed request |
fabian101 | 6:a4440eec3652 | 86 | // motorValue is a value [-1,1], currently only 0 or 1/-1 |
fabian101 | 6:a4440eec3652 | 87 | if (motorValueX >= maxXValue){motorValueX = maxXValue;} // safety |
fabian101 | 6:a4440eec3652 | 88 | else if (motorValueX <= -maxXValue){motorValueX = -maxXValue;} |
fabian101 | 6:a4440eec3652 | 89 | else if ((motorValueX >= sensitivityFactor) && (motorValueX < maxXValue)){motorValueX = maxXValue;} // if threshold is met pwm is 100%, maybe quantize here |
fabian101 | 6:a4440eec3652 | 90 | else if ((motorValueX <= -sensitivityFactor) && (motorValueX > -maxXValue)){motorValueX = -maxXValue;} |
fabian101 | 6:a4440eec3652 | 91 | else {motorValueX = 0;} |
fabian101 | 6:a4440eec3652 | 92 | if (motorValueY >= maxYValue){motorValueY = maxYValue;} |
fabian101 | 6:a4440eec3652 | 93 | else if (motorValueY <= -maxYValue){motorValueY = -maxYValue;} |
fabian101 | 6:a4440eec3652 | 94 | else if ((motorValueY >= sensitivityFactor) && (motorValueY <= maxYValue)){motorValueY = maxYValue;} |
fabian101 | 6:a4440eec3652 | 95 | else if ((motorValueY <= -sensitivityFactor) && (motorValueY >= -maxYValue)){motorValueY = -maxYValue;} |
fabian101 | 6:a4440eec3652 | 96 | else {motorValueX = 0;} |
fabian101 | 6:a4440eec3652 | 97 | //pc.printf("dit moet nu value hebben als je aanspant : %f \n\r", motorValueX); |
fabian101 | 2:6b913f93bc0b | 98 | // current position of pen in Meters |
fabian101 | 6:a4440eec3652 | 99 | float currentX = (xPulses/(8400.0f))*(2*PI)*RmX; |
fabian101 | 6:a4440eec3652 | 100 | float currentY = (yPulses/(8400.0f))*(2*PI)*Rpulley; |
fabian101 | 6:a4440eec3652 | 101 | //pc.printf("currentX : %f \n\r", currentX); |
fabian101 | 1:b9005f2aabf5 | 102 | |
fabian101 | 2:6b913f93bc0b | 103 | // upcoming position of pen in Meters |
fabian101 | 2:6b913f93bc0b | 104 | //toX = currentX + motorValueX*maxSpeed*RATE; // speed is in m/s rate is in s |
fabian101 | 2:6b913f93bc0b | 105 | //toY = currentY + motorValueY*maxSpeed*RATE; |
fabian101 | 2:6b913f93bc0b | 106 | |
fabian101 | 2:6b913f93bc0b | 107 | // distance it should travel for 1 interval in Meters |
fabian101 | 2:6b913f93bc0b | 108 | float deltaX = motorValueX*maxSpeed*RATE; |
fabian101 | 2:6b913f93bc0b | 109 | float deltaY = motorValueY*maxSpeed*RATE; |
fabian101 | 6:a4440eec3652 | 110 | //pc.printf("deltaX : %f \n\r", deltaX); |
fabian101 | 2:6b913f93bc0b | 111 | // desired velocity in m/s for this interval |
fabian101 | 2:6b913f93bc0b | 112 | float toVelX = deltaX/RATE; |
fabian101 | 2:6b913f93bc0b | 113 | float toVelY = deltaY/RATE; |
fabian101 | 6:a4440eec3652 | 114 | //pc.printf("toVelX : %f \n\r", toVelX); |
fabian101 | 2:6b913f93bc0b | 115 | // set setpoint of PID controller, this is the thing you want |
fabian101 | 2:6b913f93bc0b | 116 | X_Controller.setSetPoint(toVelX); |
fabian101 | 2:6b913f93bc0b | 117 | Y_Controller.setSetPoint(toVelY); |
fabian101 | 0:d935ea6f5c25 | 118 | |
fabian101 | 2:6b913f93bc0b | 119 | // current velocity in M/s, the thing you measure |
fabian101 | 2:6b913f93bc0b | 120 | // horizontal motor |
fabian101 | 2:6b913f93bc0b | 121 | float currVelX = (currentX - prevX)/RATE; |
fabian101 | 2:6b913f93bc0b | 122 | prevX = currentX; |
fabian101 | 6:a4440eec3652 | 123 | //pc.printf("currVe;X : %f \n\r", currVelX); |
fabian101 | 2:6b913f93bc0b | 124 | X_Controller.setProcessValue(currVelX); |
fabian101 | 1:b9005f2aabf5 | 125 | |
fabian101 | 2:6b913f93bc0b | 126 | //vertical motor |
fabian101 | 2:6b913f93bc0b | 127 | float currVelY = (currentY - prevY)/RATE; |
fabian101 | 2:6b913f93bc0b | 128 | prevY = currentY; |
fabian101 | 2:6b913f93bc0b | 129 | Y_Controller.setProcessValue(currVelY); |
fabian101 | 1:b9005f2aabf5 | 130 | |
fabian101 | 4:bfdcf3da7cff | 131 | // compute the output |
fabian101 | 6:a4440eec3652 | 132 | float X_MotorOutput = 10*(X_Controller.compute()); |
fabian101 | 6:a4440eec3652 | 133 | pc.printf("X_MotorOutput : %f \n\r", X_MotorOutput); |
fabian101 | 6:a4440eec3652 | 134 | |
fabian101 | 6:a4440eec3652 | 135 | if (X_MotorOutput >= X_frictionTrheshold) // hardcoded threshold internal friction |
fabian101 | 4:bfdcf3da7cff | 136 | { // right request |
fabian101 | 4:bfdcf3da7cff | 137 | X_Direction.write(CW); |
fabian101 | 4:bfdcf3da7cff | 138 | X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1 |
fabian101 | 4:bfdcf3da7cff | 139 | } |
fabian101 | 6:a4440eec3652 | 140 | else if (X_MotorOutput <= -X_frictionTrheshold) |
fabian101 | 4:bfdcf3da7cff | 141 | { |
fabian101 | 4:bfdcf3da7cff | 142 | X_Direction.write(!CW); // left request |
fabian101 | 6:a4440eec3652 | 143 | X_Magnitude.write(fabs(X_MotorOutput)); //motor output will be negative and you need to write a pos value |
fabian101 | 2:6b913f93bc0b | 144 | } |
fabian101 | 6:a4440eec3652 | 145 | else // else the internal friction is not compensated |
fabian101 | 6:a4440eec3652 | 146 | { |
fabian101 | 6:a4440eec3652 | 147 | X_Magnitude.write(0); |
fabian101 | 6:a4440eec3652 | 148 | X_Direction.write(CW); |
fabian101 | 6:a4440eec3652 | 149 | } |
fabian101 | 6:a4440eec3652 | 150 | |
fabian101 | 4:bfdcf3da7cff | 151 | float Y_MotorOutput = Y_Controller.compute(); |
fabian101 | 6:a4440eec3652 | 152 | if (Y_MotorOutput >= Y_frictionTrheshold) |
fabian101 | 4:bfdcf3da7cff | 153 | { // up request |
fabian101 | 4:bfdcf3da7cff | 154 | Y_Direction.write(CW); |
fabian101 | 4:bfdcf3da7cff | 155 | Y_Magnitude.write(Y_MotorOutput); |
fabian101 | 4:bfdcf3da7cff | 156 | } |
fabian101 | 6:a4440eec3652 | 157 | else if (Y_MotorOutput <= -Y_frictionTrheshold) |
fabian101 | 4:bfdcf3da7cff | 158 | { |
fabian101 | 4:bfdcf3da7cff | 159 | Y_Direction.write(!CW); // down request |
fabian101 | 6:a4440eec3652 | 160 | Y_Magnitude.write(fabs(Y_MotorOutput)); |
fabian101 | 4:bfdcf3da7cff | 161 | } |
fabian101 | 6:a4440eec3652 | 162 | else |
fabian101 | 6:a4440eec3652 | 163 | { |
fabian101 | 6:a4440eec3652 | 164 | Y_Magnitude.write(0); |
fabian101 | 6:a4440eec3652 | 165 | Y_Direction.write(CW); |
fabian101 | 6:a4440eec3652 | 166 | } |
fabian101 | 6:a4440eec3652 | 167 | update_flag = false; |
fabian101 | 0:d935ea6f5c25 | 168 | } |
fabian101 | 0:d935ea6f5c25 | 169 | } |
fabian101 | 6:a4440eec3652 | 170 | motorControl.detach(); |
fabian101 | 0:d935ea6f5c25 | 171 | } |
fabian101 | 0:d935ea6f5c25 | 172 | |
fabian101 | 4:bfdcf3da7cff | 173 | void setUpdate_flag() { |
fabian101 | 4:bfdcf3da7cff | 174 | update_flag = true; |
fabian101 | 1:b9005f2aabf5 | 175 | } |
fabian101 | 1:b9005f2aabf5 | 176 | |
fabian101 | 1:b9005f2aabf5 | 177 | void calibrate() { |
fabian101 | 6:a4440eec3652 | 178 | |
fabian101 | 2:6b913f93bc0b | 179 | // GO-TO pos (0,0) |
fabian101 | 6:a4440eec3652 | 180 | while ((X_Motor.getPulses() > margin) || (Y_Motor.getPulses() > margin)){ |
fabian101 | 6:a4440eec3652 | 181 | float pulses = X_Motor.getPulses(); |
fabian101 | 6:a4440eec3652 | 182 | printf("pulsesX : %f\n\r", pulses); |
fabian101 | 6:a4440eec3652 | 183 | if (X_Motor.getPulses() > margin) |
fabian101 | 6:a4440eec3652 | 184 | { |
fabian101 | 1:b9005f2aabf5 | 185 | // go direction LEFT |
fabian101 | 6:a4440eec3652 | 186 | X_Direction.write(!CW); |
fabian101 | 6:a4440eec3652 | 187 | X_Magnitude.write(0.4); // 8 RPM with no load, maybe compensate for internal friction |
fabian101 | 6:a4440eec3652 | 188 | } |
fabian101 | 6:a4440eec3652 | 189 | else |
fabian101 | 6:a4440eec3652 | 190 | { //motormagX 0 |
fabian101 | 2:6b913f93bc0b | 191 | X_Direction.write(CW); |
fabian101 | 2:6b913f93bc0b | 192 | X_Magnitude.write(0.0); |
fabian101 | 6:a4440eec3652 | 193 | } |
fabian101 | 6:a4440eec3652 | 194 | if (Y_Motor.getPulses() > margin) |
fabian101 | 6:a4440eec3652 | 195 | { |
fabian101 | 1:b9005f2aabf5 | 196 | // go direction DOWN |
fabian101 | 6:a4440eec3652 | 197 | Y_Direction.write(!CW); |
fabian101 | 6:a4440eec3652 | 198 | Y_Magnitude.write(0.4); |
fabian101 | 6:a4440eec3652 | 199 | } |
fabian101 | 6:a4440eec3652 | 200 | else |
fabian101 | 6:a4440eec3652 | 201 | { //motormagY 0 |
fabian101 | 2:6b913f93bc0b | 202 | Y_Direction.write(CW); |
fabian101 | 2:6b913f93bc0b | 203 | Y_Magnitude.write(0.0); |
fabian101 | 6:a4440eec3652 | 204 | } |
fabian101 | 1:b9005f2aabf5 | 205 | } |
fabian101 | 4:bfdcf3da7cff | 206 | prevX = (X_Motor.getPulses()/(8400.0f))*(2*PI)*RmX; |
fabian101 | 4:bfdcf3da7cff | 207 | prevY = (X_Motor.getPulses()/(8400.0f))*(2*PI)*Rpulley; |
fabian101 | 1:b9005f2aabf5 | 208 | } |
fabian101 | 1:b9005f2aabf5 | 209 | |
fabian101 | 6:a4440eec3652 | 210 | void calibratePower(){ |
fabian101 | 6:a4440eec3652 | 211 | motorControl.attach(&setUpdate_flag, RATE); |
fabian101 | 6:a4440eec3652 | 212 | led = false; // contract now, false = on |
fabian101 | 6:a4440eec3652 | 213 | while (counter < 3000) // get 3000 samples for the max |
fabian101 | 6:a4440eec3652 | 214 | { |
fabian101 | 6:a4440eec3652 | 215 | if(update_flag) |
fabian101 | 4:bfdcf3da7cff | 216 | { |
fabian101 | 6:a4440eec3652 | 217 | float rCal = signal.readValue(1); |
fabian101 | 6:a4440eec3652 | 218 | if ( rCal > rMax) |
fabian101 | 6:a4440eec3652 | 219 | { |
fabian101 | 6:a4440eec3652 | 220 | rMax = rCal; |
fabian101 | 6:a4440eec3652 | 221 | } |
fabian101 | 6:a4440eec3652 | 222 | |
fabian101 | 6:a4440eec3652 | 223 | float lCal = signal.readValue(2); |
fabian101 | 6:a4440eec3652 | 224 | if ( lCal > lMax) |
fabian101 | 6:a4440eec3652 | 225 | { |
fabian101 | 6:a4440eec3652 | 226 | lMax = lCal; |
fabian101 | 6:a4440eec3652 | 227 | } |
fabian101 | 6:a4440eec3652 | 228 | /* |
fabian101 | 6:a4440eec3652 | 229 | float uCal = signal.readValue(3); |
fabian101 | 6:a4440eec3652 | 230 | if ( uCal > uMax) |
fabian101 | 6:a4440eec3652 | 231 | { |
fabian101 | 6:a4440eec3652 | 232 | uMax = uCal; |
fabian101 | 6:a4440eec3652 | 233 | } |
fabian101 | 6:a4440eec3652 | 234 | float dCal = signal.readValue(4); |
fabian101 | 6:a4440eec3652 | 235 | if ( dCal > dMax) |
fabian101 | 6:a4440eec3652 | 236 | { |
fabian101 | 6:a4440eec3652 | 237 | dMax = dCal; |
fabian101 | 6:a4440eec3652 | 238 | } |
fabian101 | 6:a4440eec3652 | 239 | */ |
fabian101 | 6:a4440eec3652 | 240 | counter++; |
fabian101 | 6:a4440eec3652 | 241 | //pc.printf("rCal: %f \n\r", rCal); |
fabian101 | 6:a4440eec3652 | 242 | |
fabian101 | 6:a4440eec3652 | 243 | update_flag = false; |
fabian101 | 6:a4440eec3652 | 244 | } |
fabian101 | 1:b9005f2aabf5 | 245 | } |
fabian101 | 6:a4440eec3652 | 246 | counter = 0; |
fabian101 | 6:a4440eec3652 | 247 | pc.printf("rMax : %f\n\r", rMax); |
fabian101 | 6:a4440eec3652 | 248 | pc.printf("lMax : %f\n\r", lMax); |
fabian101 | 6:a4440eec3652 | 249 | motorControl.detach(); |
fabian101 | 6:a4440eec3652 | 250 | led = true; |
fabian101 | 6:a4440eec3652 | 251 | //pc.printf("ik moet hier led weer uitzetten \n\r"); |
fabian101 | 6:a4440eec3652 | 252 | |
fabian101 | 1:b9005f2aabf5 | 253 | } |