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.h@1:b9005f2aabf5, 2016-10-21 (annotated)

Committer:
fabian101
Date:
Fri Oct 21 21:32:20 2016 +0000
Revision:
1:b9005f2aabf5
Parent:
0:d935ea6f5c25
Child:
2:6b913f93bc0b
21-10 11:32

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fabian101 0:d935ea6f5c25 1 InterruptIn calibrateButton(D2);
fabian101 1:b9005f2aabf5 2
fabian101 1:b9005f2aabf5 3 const double PI = 3.141592653589793;
fabian101 1:b9005f2aabf5 4
fabian101 1:b9005f2aabf5 5 int x = 0; // pulsecount
fabian101 1:b9005f2aabf5 6 int x_max; // ??? physically measure
fabian101 0:d935ea6f5c25 7 int y = 0;
fabian101 0:d935ea6f5c25 8 int y_max; // ???
fabian101 1:b9005f2aabf5 9
fabian101 1:b9005f2aabf5 10 double rMax; // maxAmplitude emg signal
fabian101 0:d935ea6f5c25 11 double lMax;
fabian101 0:d935ea6f5c25 12 double uMax;
fabian101 0:d935ea6f5c25 13 double dMax;
fabian101 1:b9005f2aabf5 14
fabian101 1:b9005f2aabf5 15 float minHorThreshold; // threshold emgsignal
fabian101 1:b9005f2aabf5 16 float minVerThreshold;
fabian101 1:b9005f2aabf5 17
fabian101 1:b9005f2aabf5 18 float motorValueHor = 0; // requested speed
fabian101 0:d935ea6f5c25 19 float motorValueVer = 0;
fabian101 0:d935ea6f5c25 20
fabian101 1:b9005f2aabf5 21 const float maxHorValue = 1.0f; // v1 in drawing, max speed cm/s
fabian101 1:b9005f2aabf5 22 const float maxVerValue = 1.0f; //
fabian101 1:b9005f2aabf5 23
fabian101 1:b9005f2aabf5 24 const float maxSpeed = 1.0f; // cm/s
fabian101 1:b9005f2aabf5 25
fabian101 1:b9005f2aabf5 26 const float R1 = 8.5; // radius of big driven gear in Cm
fabian101 1:b9005f2aabf5 27 const float RmHor = 0.5; // radius of small drive gear in Cm
fabian101 1:b9005f2aabf5 28 const float RmVer = 1.5915; // radius of pulley gear in Cm
fabian101 1:b9005f2aabf5 29
fabian101 1:b9005f2aabf5 30 const float maxRotSpeedHor = maxSpeed/RmHor; // w_m = v1/r2 rad/s
fabian101 1:b9005f2aabf5 31 const float maxRotSpeedVer = maxSpeed/RmVer; // w_m = v1/r1 rad/s
fabian101 1:b9005f2aabf5 32
fabian101 1:b9005f2aabf5 33 float currentX = 0; // CM
fabian101 1:b9005f2aabf5 34 float currentY = 0;
fabian101 0:d935ea6f5c25 35
fabian101 1:b9005f2aabf5 36 float toX = 0; // CM
fabian101 1:b9005f2aabf5 37 float toY = 0;
fabian101 0:d935ea6f5c25 38
fabian101 1:b9005f2aabf5 39 float currentXangle = 0; // degrees
fabian101 1:b9005f2aabf5 40 float currentYangle = 0;
fabian101 1:b9005f2aabf5 41
fabian101 1:b9005f2aabf5 42 float toangleX = 0; // degrees
fabian101 1:b9005f2aabf5 43 float toangleY = 0;
fabian101 1:b9005f2aabf5 44
fabian101 1:b9005f2aabf5 45 float Xvelrequest = 0; // degrees/sec
fabian101 1:b9005f2aabf5 46 float Yvelrequest = 0;
fabian101 1:b9005f2aabf5 47
fabian101 1:b9005f2aabf5 48 float prevX = 0; // pulses
fabian101 1:b9005f2aabf5 49 float prevY = 0;
fabian101 1:b9005f2aabf5 50
fabian101 1:b9005f2aabf5 51 const int margin = 100; // margin pulses for calibration
fabian101 0:d935ea6f5c25 52
fabian101 0:d935ea6f5c25 53 void calibrate();
fabian101 1:b9005f2aabf5 54 void calibratePower();
fabian101 0:d935ea6f5c25 55 void run();
fabian101 0:d935ea6f5c25 56 double toMotorValue(double,double);
fabian101 0:d935ea6f5c25 57 void emergencyExit();
fabian101 0:d935ea6f5c25 58 void motorOutput(double, bool);
fabian101 1:b9005f2aabf5 59 void initControllers();
fabian101 1:b9005f2aabf5 60 void initMotors();
fabian101 0:d935ea6f5c25 61
fabian101 0:d935ea6f5c25 62 SigInterpreter signal = SigInterpreter();
fabian101 0:d935ea6f5c25 63 PulseChecker pulses = PulseChecker();
fabian101 0:d935ea6f5c25 64
fabian101 1:b9005f2aabf5 65 const float Kc = 0.0;
fabian101 1:b9005f2aabf5 66 const float Ti = 0.0;
fabian101 1:b9005f2aabf5 67 const float Td = 0.0;
fabian101 1:b9005f2aabf5 68 float RATE = 1/1000; // ???
fabian101 1:b9005f2aabf5 69
fabian101 1:b9005f2aabf5 70 PwmOut horizontalMagnitude(D5); // bind to horizontal motor
fabian101 1:b9005f2aabf5 71 DigitalOut horizontalDirection(D4);
fabian101 1:b9005f2aabf5 72 PID horizontalController(Kc, Ti, Td, RATE);
fabian101 1:b9005f2aabf5 73
fabian101 1:b9005f2aabf5 74 PwmOut verticalMagnitude(D6); // bind to vertical motor
fabian101 1:b9005f2aabf5 75 DigitalOut verticalDirection(D7);
fabian101 1:b9005f2aabf5 76 PID verticalController(Kc, Ti, Td, RATE);
fabian101 1:b9005f2aabf5 77
fabian101 0:d935ea6f5c25 78 Ticker motorControl;
fabian101 0:d935ea6f5c25 79 void setFlag();
fabian101 0:d935ea6f5c25 80 bool flag = true;
fabian101 1:b9005f2aabf5 81
fabian101 1:b9005f2aabf5 82 void initMotors(){
fabian101 1:b9005f2aabf5 83 horizontalMagnitude.period_us(50);
fabian101 1:b9005f2aabf5 84 horizontalMagnitude.write(0.0);
fabian101 1:b9005f2aabf5 85 horizontalDirection.write(1);
fabian101 1:b9005f2aabf5 86
fabian101 1:b9005f2aabf5 87 verticalMagnitude.period_us(50);
fabian101 1:b9005f2aabf5 88 verticalMagnitude.write(0.0);
fabian101 1:b9005f2aabf5 89 verticalDirection.write(1);
fabian101 1:b9005f2aabf5 90 }
fabian101 1:b9005f2aabf5 91 void initControllers(){
fabian101 1:b9005f2aabf5 92 horizontalController.setInputLimits(-90.0,90.0); // fix me
fabian101 1:b9005f2aabf5 93 horizontalController.setOutputLimits(-1.0,1.0);
fabian101 1:b9005f2aabf5 94 horizontalController.setBias(0);
fabian101 1:b9005f2aabf5 95 horizontalController.setMode(0);
fabian101 1:b9005f2aabf5 96
fabian101 1:b9005f2aabf5 97 verticalController.setInputLimits(-90.0,90.0); // fix me
fabian101 1:b9005f2aabf5 98 verticalController.setOutputLimits(-1.0,1.0);
fabian101 1:b9005f2aabf5 99 verticalController.setBias(0);
fabian101 1:b9005f2aabf5 100 verticalController.setMode(0);
fabian101 1:b9005f2aabf5 101 }