Irnti Nousi / Mbed 2 deprecated Robot_control_v4

Group19-Biorobotics

Dependencies:   HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed

Fork of Robot_control_v3 by Fabian van Hummel

Controller.cpp@6:a4440eec3652, 2016-11-02 (annotated)

Committer:
fabian101
Date:
Wed Nov 02 15:39:10 2016 +0000
Revision:
6:a4440eec3652
Parent:
4:bfdcf3da7cff
Child:
8:82e38f4aa690
biorobotics group 19 , 2 november

Who changed what in which revision?

UserRevisionLine numberNew 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 }