Fabian van Hummel / Mbed 2 deprecated Robot_control_v3

biorobotics group 19 , 2 november

Dependencies:   HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed

Fork of Robot_controlv2 by Fabian van Hummel

Controller.h@2:6b913f93bc0b, 2016-10-26 (annotated)

Committer:
fabian101
Date:
Wed Oct 26 12:37:37 2016 +0000
Revision:
2:6b913f93bc0b
Parent:
1:b9005f2aabf5
Child:
4:bfdcf3da7cff
26-10 group 19

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fabian101 1:b9005f2aabf5 1 const double PI = 3.141592653589793;
fabian101 1:b9005f2aabf5 2
fabian101 2:6b913f93bc0b 3 int xPulses = 0; // pulsecount
fabian101 2:6b913f93bc0b 4 int x_max = 33068;
fabian101 2:6b913f93bc0b 5 int yPulses = 0;
fabian101 2:6b913f93bc0b 6 int y_max = 17640;
fabian101 2:6b913f93bc0b 7 int prevXpulses = 0;
fabian101 2:6b913f93bc0b 8 int prevYpulses = 0;
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 2:6b913f93bc0b 15 float motorValueX = 0; // requested speed
fabian101 2:6b913f93bc0b 16 float motorValueY = 0;
fabian101 0:d935ea6f5c25 17
fabian101 2:6b913f93bc0b 18 const float maxXValue = 1.0f; // v1 in drawing, max speed cm/s
fabian101 2:6b913f93bc0b 19 const float maxYValue = 1.0f; //
fabian101 1:b9005f2aabf5 20
fabian101 2:6b913f93bc0b 21 const float maxSpeed = 0.01f; // m/s
fabian101 0:d935ea6f5c25 22
fabian101 2:6b913f93bc0b 23 const float R1 = 0.085; // radius of big driven gear in m
fabian101 2:6b913f93bc0b 24 const float RmX = 0.012; // radius of small drive gear in m
fabian101 2:6b913f93bc0b 25 const float Rpulley = 0.015915; // radius of pulley gear in m
fabian101 1:b9005f2aabf5 26
fabian101 2:6b913f93bc0b 27 float prevX = 0;
fabian101 1:b9005f2aabf5 28 float prevY = 0;
fabian101 1:b9005f2aabf5 29
fabian101 2:6b913f93bc0b 30 double r = 0;
fabian101 2:6b913f93bc0b 31 double l = 0;
fabian101 2:6b913f93bc0b 32 double u = 0;
fabian101 2:6b913f93bc0b 33 double d = 0;
fabian101 2:6b913f93bc0b 34 int i = 1;
fabian101 2:6b913f93bc0b 35
fabian101 2:6b913f93bc0b 36 int SAMPLES_PER_AVERAGE = 10;
fabian101 1:b9005f2aabf5 37 const int margin = 100; // margin pulses for calibration
fabian101 0:d935ea6f5c25 38
fabian101 0:d935ea6f5c25 39 void calibrate();
fabian101 1:b9005f2aabf5 40 void calibratePower();
fabian101 0:d935ea6f5c25 41 void run();
fabian101 0:d935ea6f5c25 42 double toMotorValue(double,double);
fabian101 0:d935ea6f5c25 43 void emergencyExit();
fabian101 0:d935ea6f5c25 44 void motorOutput(double, bool);
fabian101 1:b9005f2aabf5 45 void initControllers();
fabian101 1:b9005f2aabf5 46 void initMotors();
fabian101 2:6b913f93bc0b 47 void initMotorvariables();
fabian101 0:d935ea6f5c25 48
fabian101 0:d935ea6f5c25 49 SigInterpreter signal = SigInterpreter();
fabian101 0:d935ea6f5c25 50 PulseChecker pulses = PulseChecker();
fabian101 0:d935ea6f5c25 51
fabian101 1:b9005f2aabf5 52 const float Kc = 0.0;
fabian101 1:b9005f2aabf5 53 const float Ti = 0.0;
fabian101 1:b9005f2aabf5 54 const float Td = 0.0;
fabian101 2:6b913f93bc0b 55 float RATE = 0.01; // rate = interval motors will be updateed
fabian101 2:6b913f93bc0b 56
fabian101 2:6b913f93bc0b 57 PwmOut X_Magnitude(D5); // bind to horizontal motor
fabian101 2:6b913f93bc0b 58 DigitalOut X_Direction(D4);
fabian101 2:6b913f93bc0b 59 PID X_Controller(Kc, Ti, Td, RATE);
fabian101 1:b9005f2aabf5 60
fabian101 2:6b913f93bc0b 61 PwmOut Y_Magnitude(D6); // bind to Ytical motor
fabian101 2:6b913f93bc0b 62 DigitalOut Y_Direction(D7);
fabian101 2:6b913f93bc0b 63 PID Y_Controller(Kc, Ti, Td, RATE);
fabian101 1:b9005f2aabf5 64
fabian101 2:6b913f93bc0b 65 InterruptIn calibrateButton(D2);
fabian101 0:d935ea6f5c25 66 Ticker motorControl;
fabian101 0:d935ea6f5c25 67 void setFlag();
fabian101 0:d935ea6f5c25 68 bool flag = true;
fabian101 2:6b913f93bc0b 69 bool calibrateFlag = true;
fabian101 2:6b913f93bc0b 70
fabian101 2:6b913f93bc0b 71 int CW = true;
fabian101 1:b9005f2aabf5 72
fabian101 1:b9005f2aabf5 73 void initMotors(){
fabian101 2:6b913f93bc0b 74 X_Magnitude.period_us(50);
fabian101 2:6b913f93bc0b 75 X_Magnitude.write(0.0);
fabian101 2:6b913f93bc0b 76 X_Direction.write(CW);
fabian101 1:b9005f2aabf5 77
fabian101 2:6b913f93bc0b 78 Y_Magnitude.period_us(50);
fabian101 2:6b913f93bc0b 79 Y_Magnitude.write(0.0);
fabian101 2:6b913f93bc0b 80 Y_Direction.write(CW);
fabian101 1:b9005f2aabf5 81 }
fabian101 1:b9005f2aabf5 82 void initControllers(){
fabian101 2:6b913f93bc0b 83 X_Controller.setInputLimits(-90.0,90.0); // fiX_ me
fabian101 2:6b913f93bc0b 84 X_Controller.setOutputLimits(-1.0,1.0);
fabian101 2:6b913f93bc0b 85 X_Controller.setBias(0);
fabian101 2:6b913f93bc0b 86 X_Controller.setMode(0);
fabian101 1:b9005f2aabf5 87
fabian101 2:6b913f93bc0b 88 Y_Controller.setInputLimits(-90.0,90.0); // fix me
fabian101 2:6b913f93bc0b 89 Y_Controller.setOutputLimits(-1.0,1.0);
fabian101 2:6b913f93bc0b 90 Y_Controller.setBias(0);
fabian101 2:6b913f93bc0b 91 Y_Controller.setMode(0);
fabian101 1:b9005f2aabf5 92 }