Fabian van Hummel / Mbed 2 deprecated Robot_controlv2

writing mechaniscm 28-10

Dependencies:   HIDScope MODSERIAL PID QEI SigInterpreterv2 biquadFilter mbed

Fork of Robot_control by Fabian van Hummel

Controller.cpp@5:623bab832e6a, 2016-10-28 (annotated)

Committer:
fabian101
Date:
Fri Oct 28 12:50:44 2016 +0000
Revision:
5:623bab832e6a
Parent:
4:bfdcf3da7cff 
28-10, filtering needs tweaking, pid control needs testing, minor adjustments

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