Peter Knoben / Mbed 2 deprecated ShowIt_robot_control

Final Version

Dependencies:   AnglePosition2 Encoder FastPWM MovementClean PIDController Servo SignalNumberClean biquadFilter mbed

Fork of ShowItv2 by Peter Knoben

main.cpp@3:c768a37620c9, 2017-10-30 (annotated)

Committer:
peterknoben
Date:
Mon Oct 30 10:43:26 2017 +0000
Revision:
3:c768a37620c9
Parent:
1:406519ff0f17
Child:
4:e15fc329b88b
kaldf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
peterknoben 3:c768a37620c9 1 #include "MODSERIAL.h"
peterknoben 1:406519ff0f17 2 #include "AnglePosition.h"
peterknoben 1:406519ff0f17 3 #include "PIDControl.h"
peterknoben 3:c768a37620c9 4 #include "BiQuad.h"
peterknoben 3:c768a37620c9 5 #include "signalnumber.h"
DBerendsen 0:5f4bc2d63814 6 #include "mbed.h"
DBerendsen 0:5f4bc2d63814 7 #include "encoder.h"
DBerendsen 0:5f4bc2d63814 8 #include "Servo.h"
peterknoben 1:406519ff0f17 9 #include "FastPWM.h"
DBerendsen 0:5f4bc2d63814 10
DBerendsen 0:5f4bc2d63814 11 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 12 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 13 //------------------------------------------------------------------------------
peterknoben 3:c768a37620c9 14 MODSERIAL pc(USBTX, USBRX); //Establish connection
peterknoben 1:406519ff0f17 15 Ticker MyControllerTicker1; //Declare Ticker Motor 1
peterknoben 1:406519ff0f17 16 Ticker MyControllerTicker2; //Declare Ticker Motor 2
peterknoben 3:c768a37620c9 17 Ticker MySampleTicker; //Declare Ticker HIDscope
peterknoben 3:c768a37620c9 18 Ticker MyTickerMean; //Declare Ticker Signalprocessing
peterknoben 3:c768a37620c9 19
peterknoben 3:c768a37620c9 20 InterruptIn But2(PTC6); //Declare button for calibration
peterknoben 3:c768a37620c9 21
peterknoben 1:406519ff0f17 22 AnglePosition Angle; //Declare Angle calculater
peterknoben 1:406519ff0f17 23 PIDControl PID; //Declare PID Controller
peterknoben 3:c768a37620c9 24 SignalNumber Signal; //Declare Signal determiner
peterknoben 3:c768a37620c9 25
peterknoben 3:c768a37620c9 26 AnalogIn potmeter1(A5);
peterknoben 3:c768a37620c9 27 AnalogIn potmeter5(A3); //Set Inputpin
peterknoben 1:406519ff0f17 28 AnalogIn potmeter2(A4); //Set Inputpin
peterknoben 3:c768a37620c9 29 AnalogIn emg0( A0 ); //Set Inputpin
peterknoben 3:c768a37620c9 30 AnalogIn emg1( A1 ); //Set Inputpin
peterknoben 3:c768a37620c9 31
peterknoben 1:406519ff0f17 32
peterknoben 1:406519ff0f17 33 const float CONTROLLER_TS = 0.02; //Motor frequency
peterknoben 3:c768a37620c9 34 const float MEAN_TS = 0.1; //Mean value frequency
peterknoben 3:c768a37620c9 35 const float SAMPLE_TS = 0.02; //BiQuad frequency
peterknoben 3:c768a37620c9 36
peterknoben 3:c768a37620c9 37
peterknoben 3:c768a37620c9 38 //------------------------------------------------------------------------------
peterknoben 3:c768a37620c9 39 //-----------------------------EMG Signals--------------------------------------
peterknoben 3:c768a37620c9 40 //------------------------------------------------------------------------------
peterknoben 3:c768a37620c9 41 const int n = 10; //Window size for the mean value, also adjust in signalnumber.cpp
peterknoben 3:c768a37620c9 42 const int action =50; //Number of same mean values to change the signalnumber
peterknoben 3:c768a37620c9 43 const int m = 10; //Number of samples for calibration
peterknoben 3:c768a37620c9 44 int mode;
peterknoben 3:c768a37620c9 45 float emg_offset;
peterknoben 3:c768a37620c9 46 float meanx;
peterknoben 3:c768a37620c9 47
peterknoben 3:c768a37620c9 48 //BiQuad filter
peterknoben 3:c768a37620c9 49 BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
peterknoben 3:c768a37620c9 50 BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
peterknoben 3:c768a37620c9 51 BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
peterknoben 3:c768a37620c9 52 BiQuadChain BiQuad_filter;
peterknoben 3:c768a37620c9 53
peterknoben 3:c768a37620c9 54 // Calibration function
peterknoben 3:c768a37620c9 55 void calibration(){
peterknoben 3:c768a37620c9 56 emg_offset = Signal.calibrate(m, potmeter5);
peterknoben 3:c768a37620c9 57 pc.printf("offset = %f \r\n", emg_offset);
peterknoben 3:c768a37620c9 58 }
peterknoben 3:c768a37620c9 59
peterknoben 3:c768a37620c9 60 /*
peterknoben 3:c768a37620c9 61 //Sample function
peterknoben 3:c768a37620c9 62 void sample()
peterknoben 3:c768a37620c9 63 {
peterknoben 3:c768a37620c9 64 float Signal=((emg0+emg1)/2)-emg_offset;
peterknoben 3:c768a37620c9 65 float Signal_filtered= BiQuad_filter.step(Signal);
peterknoben 3:c768a37620c9 66 /* scope
peterknoben 3:c768a37620c9 67 scope.set(0, emg0.read() );
peterknoben 3:c768a37620c9 68 scope.set(1, emg1.read() );
peterknoben 3:c768a37620c9 69 scope.set(2, Signal_filtered);
peterknoben 3:c768a37620c9 70 scope.send();
peterknoben 3:c768a37620c9 71 led = !led;
peterknoben 3:c768a37620c9 72 /
peterknoben 3:c768a37620c9 73 }
peterknoben 3:c768a37620c9 74 */
peterknoben 3:c768a37620c9 75
peterknoben 3:c768a37620c9 76 float Filter(float input0, float input1, float offset){
peterknoben 3:c768a37620c9 77 float Signal=((input0+input1)/2)-offset;
peterknoben 3:c768a37620c9 78 float Signal_filtered= BiQuad_filter.step(Signal);
peterknoben 3:c768a37620c9 79 return Signal_filtered;
peterknoben 3:c768a37620c9 80 }
peterknoben 3:c768a37620c9 81
peterknoben 3:c768a37620c9 82 void signalnumber(){
peterknoben 3:c768a37620c9 83 float Signal_filtered = Filter(emg0, emg1, emg_offset);
peterknoben 3:c768a37620c9 84 meanx = Signal.getmean(n, Signal_filtered);
peterknoben 3:c768a37620c9 85 mode = Signal.getnumber(n, action, Signal_filtered);
peterknoben 3:c768a37620c9 86 pc.printf("pot = %d after filtering = %f mean = %f Signalnumber = %i \r\n" , potmeter5, meanx, mode);
peterknoben 3:c768a37620c9 87 }
peterknoben 3:c768a37620c9 88
peterknoben 1:406519ff0f17 89
peterknoben 1:406519ff0f17 90 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 91 //-------------------------Kinematic Constants----------------------------------
peterknoben 1:406519ff0f17 92 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 93 const double RAD_PER_PULSE = 0.00074799825*2;
DBerendsen 0:5f4bc2d63814 94 const double PI = 3.14159265358979323846;
peterknoben 1:406519ff0f17 95 const float max_rangex = 500.0;
peterknoben 1:406519ff0f17 96 const float max_rangey = 300.0;
peterknoben 1:406519ff0f17 97 const float x_offset = 156.15;
peterknoben 1:406519ff0f17 98 const float y_offset = -76.97;
peterknoben 1:406519ff0f17 99 const float alpha_offset = -(21.52/180)*PI;
peterknoben 1:406519ff0f17 100 const float beta_offset = 0.0;
peterknoben 1:406519ff0f17 101 const float L1 = 450.0;
peterknoben 1:406519ff0f17 102 const float L2 = 490.0;
DBerendsen 0:5f4bc2d63814 103
DBerendsen 0:5f4bc2d63814 104
DBerendsen 0:5f4bc2d63814 105 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 106 //--------------------------------Servo-----------------------------------------
DBerendsen 0:5f4bc2d63814 107 //------------------------------------------------------------------------------
peterknoben 3:c768a37620c9 108 // This will be programmed on a different Mbed
peterknoben 1:406519ff0f17 109 Servo MyServo(D9); //Declare button
peterknoben 1:406519ff0f17 110 InterruptIn But1(D8); //Declare used button
peterknoben 1:406519ff0f17 111 int k=0; //Position flag
DBerendsen 0:5f4bc2d63814 112
DBerendsen 0:5f4bc2d63814 113 void servo_control (){
DBerendsen 0:5f4bc2d63814 114 if (k==0){
DBerendsen 0:5f4bc2d63814 115 MyServo = 0;
DBerendsen 0:5f4bc2d63814 116 k=1;
DBerendsen 0:5f4bc2d63814 117 }
DBerendsen 0:5f4bc2d63814 118 else{
DBerendsen 0:5f4bc2d63814 119 MyServo = 2;
DBerendsen 0:5f4bc2d63814 120 k=0;
peterknoben 1:406519ff0f17 121 }
DBerendsen 0:5f4bc2d63814 122 }
DBerendsen 0:5f4bc2d63814 123
DBerendsen 0:5f4bc2d63814 124
DBerendsen 0:5f4bc2d63814 125 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 126 //--------------------------------Motor1----------------------------------------
DBerendsen 0:5f4bc2d63814 127 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 128 FastPWM motor1(D5);
DBerendsen 0:5f4bc2d63814 129 DigitalOut motor1DirectionPin(D4);
DBerendsen 0:5f4bc2d63814 130 DigitalIn ENC2A(D12);
DBerendsen 0:5f4bc2d63814 131 DigitalIn ENC2B(D13);
DBerendsen 0:5f4bc2d63814 132 Encoder encoder1(D13,D12);
peterknoben 1:406519ff0f17 133 const float MOTOR1_KP = 40.0;
peterknoben 1:406519ff0f17 134 const float MOTOR1_KI = 0.0;
peterknoben 1:406519ff0f17 135 const float MOTOR1_KD = 15.0;
peterknoben 1:406519ff0f17 136 double M1_v1 = 0.0;
peterknoben 1:406519ff0f17 137 double M1_v2 = 0.0;
DBerendsen 0:5f4bc2d63814 138 const double motor1_gain = 2*PI;
peterknoben 1:406519ff0f17 139 const float M1_N = 0.5;
DBerendsen 0:5f4bc2d63814 140
DBerendsen 0:5f4bc2d63814 141
DBerendsen 0:5f4bc2d63814 142 void motor1_control(){
peterknoben 1:406519ff0f17 143 float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, potmeter1, potmeter2);
DBerendsen 0:5f4bc2d63814 144 float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
DBerendsen 0:5f4bc2d63814 145 float error_alpha = reference_alpha-position_alpha;
peterknoben 1:406519ff0f17 146 float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
DBerendsen 0:5f4bc2d63814 147 motor1 = fabs(magnitude1);
peterknoben 3:c768a37620c9 148 // pc.printf("err_a = %f alpha = %f mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
peterknoben 3:c768a37620c9 149 // pc.printf("\r\n");
peterknoben 1:406519ff0f17 150 // Determine Motor Direction
peterknoben 1:406519ff0f17 151 if (magnitude1 < 0){
DBerendsen 0:5f4bc2d63814 152 motor1DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 153 }
DBerendsen 0:5f4bc2d63814 154 else{
DBerendsen 0:5f4bc2d63814 155 motor1DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 156 }
DBerendsen 0:5f4bc2d63814 157 }
DBerendsen 0:5f4bc2d63814 158
DBerendsen 0:5f4bc2d63814 159 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 160 //--------------------------------Motor2----------------------------------------
DBerendsen 0:5f4bc2d63814 161 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 162 FastPWM motor2(D6);
DBerendsen 0:5f4bc2d63814 163 DigitalOut motor2DirectionPin(D7);
DBerendsen 0:5f4bc2d63814 164 DigitalIn ENC1A(D10);
DBerendsen 0:5f4bc2d63814 165 DigitalIn ENC1B(D11);
DBerendsen 0:5f4bc2d63814 166 Encoder encoder2(D10,D11);
peterknoben 1:406519ff0f17 167 const float MOTOR2_KP = 60.0;
peterknoben 1:406519ff0f17 168 const float MOTOR2_KI = 0.0;
peterknoben 1:406519ff0f17 169 const float MOTOR2_KD = 15.0;
DBerendsen 0:5f4bc2d63814 170 double m2_err_int = 0;
DBerendsen 0:5f4bc2d63814 171 const double motor2_gain = 2*PI;
peterknoben 1:406519ff0f17 172 const float M2_N = 0.5;
peterknoben 1:406519ff0f17 173 double M2_v1 = 0.0;
peterknoben 1:406519ff0f17 174 double M2_v2 = 0.0;
DBerendsen 0:5f4bc2d63814 175
DBerendsen 0:5f4bc2d63814 176
DBerendsen 0:5f4bc2d63814 177 void motor2_control(){
peterknoben 1:406519ff0f17 178 float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, potmeter1, potmeter2);
DBerendsen 0:5f4bc2d63814 179 float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
DBerendsen 0:5f4bc2d63814 180 float error_beta = reference_beta-position_beta;
peterknoben 1:406519ff0f17 181 float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
DBerendsen 0:5f4bc2d63814 182 motor2 = fabs(magnitude2);
peterknoben 3:c768a37620c9 183 // pc.printf("err2 = %f beta = %f mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
peterknoben 3:c768a37620c9 184 // pc.printf("\r\n");
peterknoben 1:406519ff0f17 185 //Determine Motor Direction
DBerendsen 0:5f4bc2d63814 186 if (magnitude2 > 0){
DBerendsen 0:5f4bc2d63814 187 motor2DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 188 }
DBerendsen 0:5f4bc2d63814 189 else{
DBerendsen 0:5f4bc2d63814 190 motor2DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 191 }
DBerendsen 0:5f4bc2d63814 192 }
DBerendsen 0:5f4bc2d63814 193 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 194 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 195 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 196
DBerendsen 0:5f4bc2d63814 197 int main(){
DBerendsen 0:5f4bc2d63814 198 pc.baud(115200);
peterknoben 3:c768a37620c9 199 BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3);
peterknoben 1:406519ff0f17 200 motor1.period(0.0001f);
peterknoben 1:406519ff0f17 201 motor2.period(0.0001f);
DBerendsen 0:5f4bc2d63814 202 MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS);
DBerendsen 0:5f4bc2d63814 203 MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
peterknoben 3:c768a37620c9 204 MyTickerMean.attach(&signalnumber, MEAN_TS);
peterknoben 3:c768a37620c9 205 // MySampleTicker.attach(&sample, SAMPLE_TS);
peterknoben 1:406519ff0f17 206 But1.rise(&servo_control);
peterknoben 3:c768a37620c9 207 But2.rise(&calibration);
DBerendsen 0:5f4bc2d63814 208 while(1) {}
peterknoben 1:406519ff0f17 209 }
peterknoben 1:406519ff0f17 210
peterknoben 1:406519ff0f17 211