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@8:82e38f4aa690, 2016-11-07 (annotated)

Committer:
AlexGroup19
Date:
Mon Nov 07 11:29:28 2016 +0000
Revision:
8:82e38f4aa690
Parent:
6:a4440eec3652 
Group19-Biorobotics

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