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@0:d935ea6f5c25, 2016-10-21 (annotated)

Committer:
fabian101
Date:
Fri Oct 21 09:13:53 2016 +0000
Revision:
0:d935ea6f5c25
Child:
1:b9005f2aabf5
robot control 21-10 11:13;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fabian101 0:d935ea6f5c25 1 #include "initialize.h"
fabian101 0:d935ea6f5c25 2 #include "Controller.h"
fabian101 0:d935ea6f5c25 3 int main() {
fabian101 0:d935ea6f5c25 4 calibrateButton.rise(&calibrate);
fabian101 0:d935ea6f5c25 5 motorControl.attach(&setFlag, RATE);
fabian101 0:d935ea6f5c25 6 //[FIXME]calibratePowerrrr(); store max amplitude each emg signal
fabian101 0:d935ea6f5c25 7 run();
fabian101 0:d935ea6f5c25 8 }
fabian101 0:d935ea6f5c25 9
fabian101 0:d935ea6f5c25 10 void run() {
fabian101 0:d935ea6f5c25 11 while(true){
fabian101 0:d935ea6f5c25 12 double r = signal.readValue(1); // returns filtered value
fabian101 0:d935ea6f5c25 13 double l = signal.readValue(2);
fabian101 0:d935ea6f5c25 14 double u = signal.readValue(3);
fabian101 0:d935ea6f5c25 15 double d = signal.readValue(4);
fabian101 0:d935ea6f5c25 16
fabian101 0:d935ea6f5c25 17 // calculate r,l,u,d in a % of max
fabian101 0:d935ea6f5c25 18 double percentageR = r/rMax;
fabian101 0:d935ea6f5c25 19 double percentageL = l/lMax;
fabian101 0:d935ea6f5c25 20 double percentageU = u/uMax;
fabian101 0:d935ea6f5c25 21 double percentageD = d/dMax;
fabian101 0:d935ea6f5c25 22
fabian101 0:d935ea6f5c25 23
fabian101 0:d935ea6f5c25 24 if (flag) {
fabian101 0:d935ea6f5c25 25 // ignore the weaker signal and motorValue is calculate here
fabian101 0:d935ea6f5c25 26 if ( percentageR > percentageL){ // horizontal speed
fabian101 0:d935ea6f5c25 27 motorValueHor = percentageR;
fabian101 0:d935ea6f5c25 28 } else {motorValueHor = -percentageL;}
fabian101 0:d935ea6f5c25 29 if (percentageU > percentageD) { // vertical speed
fabian101 0:d935ea6f5c25 30 motorValueVer = percentageU;
fabian101 0:d935ea6f5c25 31 } else {motorValueVer = -percentageD;}
fabian101 0:d935ea6f5c25 32
fabian101 0:d935ea6f5c25 33 // current pulses and prevpulses calculated
fabian101 0:d935ea6f5c25 34 x += (pulses.getDistanceHor() - prevX); // in pulses
fabian101 0:d935ea6f5c25 35 prevX = x;
fabian101 0:d935ea6f5c25 36 y += (pulses.getDistanceVer() - prevY);
fabian101 0:d935ea6f5c25 37 prevY = y;
fabian101 0:d935ea6f5c25 38
fabian101 0:d935ea6f5c25 39 //edge detection: for now it will calibrate; implement function that allows movement for the other directions
fabian101 0:d935ea6f5c25 40 if (x > x_max || x < 0) {
fabian101 0:d935ea6f5c25 41 emergencyExit();
fabian101 0:d935ea6f5c25 42 }
fabian101 0:d935ea6f5c25 43 if (y > y_max || y < 0) { //
fabian101 0:d935ea6f5c25 44 emergencyExit();
fabian101 0:d935ea6f5c25 45 }
fabian101 0:d935ea6f5c25 46
fabian101 0:d935ea6f5c25 47 // Quantize
fabian101 0:d935ea6f5c25 48 if (motorValueHor > maxHorVel){motorValueHor = maxHorVel;}
fabian101 0:d935ea6f5c25 49 if (motorValueHor < -maxHorVel){motorValueHor = -maxHorVel;}
fabian101 0:d935ea6f5c25 50 if (motorValueHor > 0 && motorValueHor < maxHorVel){motorValueHor = 0.5*maxHorVel;}
fabian101 0:d935ea6f5c25 51 if (motorValueHor < 0 && motorValueHor > -maxHorVel){motorValueHor = -0.5*maxHorVel;}
fabian101 0:d935ea6f5c25 52
fabian101 0:d935ea6f5c25 53 if ( motorValueVer > maxVerVel){motorValueVer = 1;}
fabian101 0:d935ea6f5c25 54 if (motorValueVer < -1){motorValueVer = -1;}
fabian101 0:d935ea6f5c25 55 if (motorValueVer > 0 && motorValueVer < 1){motorValueVer = 0.5;}
fabian101 0:d935ea6f5c25 56 if (motorValueVer < 0 && motorValueVer > -1){motorValueVer = -0.5;}
fabian101 0:d935ea6f5c25 57
fabian101 0:d935ea6f5c25 58 // Calculate desired rotational velocity from linear velocity
fabian101 0:d935ea6f5c25 59
fabian101 0:d935ea6f5c25 60 // Calculate current rotational velocity
fabian101 0:d935ea6f5c25 61
fabian101 0:d935ea6f5c25 62 // give the PID controller the setpoint and processvariable
fabian101 0:d935ea6f5c25 63
fabian101 0:d935ea6f5c25 64 // compute PID
fabian101 0:d935ea6f5c25 65
fabian101 0:d935ea6f5c25 66 // output to motor
fabian101 0:d935ea6f5c25 67 // motorOutput(toMotorValue(percentageR, percentageL), true); // [FIXME]- safety edges (if statement)
fabian101 0:d935ea6f5c25 68 // motorOutput(toMotorValue(percentageU, percentageD), false); // replace this with all the calculations and PID class stuff
fabian101 0:d935ea6f5c25 69
fabian101 0:d935ea6f5c25 70 // output to hidscope
fabian101 0:d935ea6f5c25 71 flag = false; // only output to motor with RATE
fabian101 0:d935ea6f5c25 72 }
fabian101 0:d935ea6f5c25 73
fabian101 0:d935ea6f5c25 74 }
fabian101 0:d935ea6f5c25 75 }
fabian101 0:d935ea6f5c25 76 void calibrate() {
fabian101 0:d935ea6f5c25 77 // GO-TO pos (0,0)
fabian101 0:d935ea6f5c25 78 // pulses can only be 0 or positive so reverse the direction until 0 pulses is reached
fabian101 0:d935ea6f5c25 79 // problem with the interrupt button, after calibration the program should re-start the run function instead of going further at the point it left
fabian101 0:d935ea6f5c25 80 // this will cause 1 iteration to be absolutely wrong -> consequences?
fabian101 0:d935ea6f5c25 81 return;
fabian101 0:d935ea6f5c25 82 }
fabian101 0:d935ea6f5c25 83
fabian101 0:d935ea6f5c25 84 void motorOutput(double motorValue, bool horizontal) { // IMPLEMENT DEADZONE?
fabian101 0:d935ea6f5c25 85 if (motorValue > 1) { // safety
fabian101 0:d935ea6f5c25 86 motorValue = 1;
fabian101 0:d935ea6f5c25 87 }
fabian101 0:d935ea6f5c25 88 if (motorValue < -1) {
fabian101 0:d935ea6f5c25 89 motorValue = -1;
fabian101 0:d935ea6f5c25 90 }
fabian101 0:d935ea6f5c25 91
fabian101 0:d935ea6f5c25 92 // deadzone here
fabian101 0:d935ea6f5c25 93
fabian101 0:d935ea6f5c25 94 if (horizontal) {
fabian101 0:d935ea6f5c25 95 motor.outputHor(fabs(motorValue), (motorValue>0) );
fabian101 0:d935ea6f5c25 96 } else {
fabian101 0:d935ea6f5c25 97 motor.outputVer(fabs(motorValue), (motorValue>0));
fabian101 0:d935ea6f5c25 98 }
fabian101 0:d935ea6f5c25 99 }
fabian101 0:d935ea6f5c25 100
fabian101 0:d935ea6f5c25 101 void setFlag() {
fabian101 0:d935ea6f5c25 102 flag = true;
fabian101 0:d935ea6f5c25 103 }
fabian101 0:d935ea6f5c25 104
fabian101 0:d935ea6f5c25 105 void emergencyExit() {
fabian101 0:d935ea6f5c25 106 calibrate();
fabian101 0:d935ea6f5c25 107 run();
fabian101 0:d935ea6f5c25 108 }