Leon Klute / Mbed 2 deprecated EMG_Controller_6

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of EMG_Controller_5 by Nahuel Manterola

main.cpp@14:a27de0ddf09f, 2016-10-28 (annotated)

Committer:
NahuelM
Date:
Fri Oct 28 12:40:06 2016 +0000
Revision:
14:a27de0ddf09f
Parent:
13:0adbf6a5de37
Child:
15:4cd14d429ad8
Volledig werkend systeem

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NahuelM 0:46582bba07d2 1 #include "mbed.h"
NahuelM 0:46582bba07d2 2 #include "QEI.h"
NahuelM 3:1d43dd4f37eb 3
NahuelM 4:e59a99c5aa08 4 #include "servoController.h"
NahuelM 2:57523bb4e9c6 5 #include "emg.h"
NahuelM 2:57523bb4e9c6 6
NahuelM 1:83fccd7d8483 7 #define pi 3.14159265359;
NahuelM 0:46582bba07d2 8
NahuelM 0:46582bba07d2 9 PwmOut Motor1_pwm(D5);
NahuelM 1:83fccd7d8483 10 DigitalOut SlideMotor_Direction(D4);
NahuelM 1:83fccd7d8483 11 PwmOut Motor2_pwm(D6);
NahuelM 1:83fccd7d8483 12 DigitalOut LiftMotor_Direction(D7);
NahuelM 1:83fccd7d8483 13 AnalogIn Potmeter(A0);
NahuelM 2:57523bb4e9c6 14 AnalogIn Potmeter2(A1);
NahuelM 1:83fccd7d8483 15 QEI Slide_Encoder(D12,D13,NC,64);
NahuelM 1:83fccd7d8483 16 QEI Lift_Encoder(D10,D11,NC,64);
NahuelM 11:c8b6a2b314c3 17
NahuelM 11:c8b6a2b314c3 18 BiQuadChain Hz_1;
NahuelM 11:c8b6a2b314c3 19 BiQuad bq0(0.05852855368, 0.11705710736, 0.05852855368,-1.05207469728, 0.28586907478);
NahuelM 11:c8b6a2b314c3 20 BiQuad bq1(0.06463239794, 0.12926479589, 0.06463239794,-1.16338171052, 0.42191097989);
NahuelM 11:c8b6a2b314c3 21 BiQuad bq2(0.07902502847, 0.15805005694, 0.07902502847,-1.42439823874, 0.7409311811);
NahuelM 11:c8b6a2b314c3 22
NahuelM 4:e59a99c5aa08 23 //Serial pc(USBTX,USBRX);
NahuelM 0:46582bba07d2 24 Ticker Controller;
NahuelM 0:46582bba07d2 25
NahuelM 1:83fccd7d8483 26 bool Controller_Flag=0;
LeeJon 7:eed677b636d3 27 float Frequency = 30;
NahuelM 0:46582bba07d2 28 float Frequency_PWM = 10000;
NahuelM 1:83fccd7d8483 29
NahuelM 1:83fccd7d8483 30 float Slide_Radius = 12.5;
NahuelM 1:83fccd7d8483 31 float Slide_Multiplier = 1;
NahuelM 1:83fccd7d8483 32 float k1 = 1;
NahuelM 12:12c162dc8893 33 float k2 = 0.1f;
NahuelM 1:83fccd7d8483 34 float k3 = 0.1f;
NahuelM 1:83fccd7d8483 35 float Start_slow = 40;
NahuelM 1:83fccd7d8483 36 float Start_lock = 0;
NahuelM 1:83fccd7d8483 37 float End_slow = 340;
NahuelM 1:83fccd7d8483 38 float End_lock = 380;
NahuelM 1:83fccd7d8483 39 float Slide_Counts;
NahuelM 1:83fccd7d8483 40 float Slide_Revolutions;
NahuelM 1:83fccd7d8483 41 float Slide_Angle;
NahuelM 1:83fccd7d8483 42 float Slide_Position;
NahuelM 0:46582bba07d2 43
NahuelM 1:83fccd7d8483 44 float Slide_Input_force = 0;
NahuelM 1:83fccd7d8483 45 float Slide_Curr_speed = 0;
NahuelM 1:83fccd7d8483 46 float Slide_Desired_speed;
NahuelM 1:83fccd7d8483 47 float Slide_Delta_speed;
NahuelM 1:83fccd7d8483 48 float Slide_Int_delta_speed;
NahuelM 1:83fccd7d8483 49 float Slide_Deriv_delta_speed = 0;
NahuelM 1:83fccd7d8483 50 float Slide_Prev_delta_speed = 0;
NahuelM 1:83fccd7d8483 51 float Slide_PI;
NahuelM 1:83fccd7d8483 52
NahuelM 1:83fccd7d8483 53 float Lift_Radius = 10;
NahuelM 1:83fccd7d8483 54 float Lift_Multiplier = 1;
NahuelM 11:c8b6a2b314c3 55 float Lift_k1 = 0.2;
NahuelM 1:83fccd7d8483 56 float Lift_k2 = 0.05;
NahuelM 1:83fccd7d8483 57 float Lift_k3 = 0.01;
NahuelM 1:83fccd7d8483 58 float Lift_Start = 0;
NahuelM 1:83fccd7d8483 59 float Lift_End = 50;
NahuelM 1:83fccd7d8483 60 float Lift_Counts;
NahuelM 1:83fccd7d8483 61 float Lift_Revolutions;
NahuelM 1:83fccd7d8483 62 float Lift_Angle;
NahuelM 1:83fccd7d8483 63 float Lift_Position;
NahuelM 1:83fccd7d8483 64
NahuelM 1:83fccd7d8483 65 float Lift_Input_force = 0;
NahuelM 1:83fccd7d8483 66 float Lift_Desired_position;
NahuelM 1:83fccd7d8483 67 float Lift_Delta_position;
NahuelM 1:83fccd7d8483 68 float Lift_Int_delta_position;
NahuelM 1:83fccd7d8483 69 float Lift_Deriv_delta_position = 0;
NahuelM 1:83fccd7d8483 70 float Lift_Prev_delta_position = 0;
NahuelM 1:83fccd7d8483 71 float Lift_PI;
NahuelM 12:12c162dc8893 72 bool Lift_Switch = false;
NahuelM 12:12c162dc8893 73 Timeout Lift_Timeout;
NahuelM 12:12c162dc8893 74 bool Lift_Timeout_Switch = true;
NahuelM 0:46582bba07d2 75
NahuelM 0:46582bba07d2 76 void Slide_Controller();
NahuelM 1:83fccd7d8483 77 void Lift_Controller();
NahuelM 1:83fccd7d8483 78 void Ticker_Flag();
NahuelM 0:46582bba07d2 79
LeeJon 13:0adbf6a5de37 80 float statechange_threshold = 0.6; /////// moet gekopieerd worden t/m-->
LeeJon 13:0adbf6a5de37 81 bool binary_norm_emg_2 = 0;
LeeJon 13:0adbf6a5de37 82 bool binary_norm_emg_2_previous = 0;
LeeJon 13:0adbf6a5de37 83 states scoopstate = STATE_MOVE;
LeeJon 13:0adbf6a5de37 84 bool state_resettime_value = 1;
LeeJon 13:0adbf6a5de37 85 Timeout state_resettime;
LeeJon 13:0adbf6a5de37 86 DigitalOut LED_STATE_MOVE(D15);
LeeJon 13:0adbf6a5de37 87 DigitalOut LED_STATE_GRAB(D14);
LeeJon 13:0adbf6a5de37 88 Timeout controlstarterTimeout;
LeeJon 13:0adbf6a5de37 89
LeeJon 13:0adbf6a5de37 90 void set2true(){
LeeJon 13:0adbf6a5de37 91 state_resettime_value = 1;
LeeJon 13:0adbf6a5de37 92 }
LeeJon 13:0adbf6a5de37 93 void change_scoopstate(){
NahuelM 14:a27de0ddf09f 94 Motor1_pwm.write(0);
NahuelM 14:a27de0ddf09f 95 Motor2_pwm.write(0);
LeeJon 13:0adbf6a5de37 96 switch(scoopstate){
LeeJon 13:0adbf6a5de37 97 case STATE_MOVE:
LeeJon 13:0adbf6a5de37 98 scoopstate = STATE_GRAB;
LeeJon 13:0adbf6a5de37 99 break;
LeeJon 13:0adbf6a5de37 100 case STATE_GRAB:
LeeJon 13:0adbf6a5de37 101 scoopstate = STATE_MOVE;
LeeJon 13:0adbf6a5de37 102 break;
LeeJon 13:0adbf6a5de37 103 }
LeeJon 13:0adbf6a5de37 104 state_resettime.attach(&set2true, 1);
LeeJon 13:0adbf6a5de37 105 state_resettime_value = 0;
LeeJon 13:0adbf6a5de37 106 }
LeeJon 13:0adbf6a5de37 107 void start_controlling(){
LeeJon 13:0adbf6a5de37 108 Controller.attach(&Ticker_Flag,1/Frequency);
LeeJon 13:0adbf6a5de37 109 } ///////////////hier
LeeJon 13:0adbf6a5de37 110
NahuelM 0:46582bba07d2 111 int main()
NahuelM 0:46582bba07d2 112 {
NahuelM 0:46582bba07d2 113 Motor1_pwm.period(1.0/Frequency_PWM);//T=1/f
NahuelM 1:83fccd7d8483 114 Motor2_pwm.period(1.0/Frequency_PWM);//T=1/f
LeeJon 13:0adbf6a5de37 115
NahuelM 0:46582bba07d2 116 pc.baud(9600);
NahuelM 2:57523bb4e9c6 117
pbaardwijk 6:6cb7c0247560 118 led.write(1);
NahuelM 3:1d43dd4f37eb 119 Lift_Input_force = Potmeter.read();
NahuelM 3:1d43dd4f37eb 120 Slide_Input_force = Potmeter2.read();
NahuelM 3:1d43dd4f37eb 121
NahuelM 12:12c162dc8893 122 notch_50_0.add( &bq03 ).add( &bq04 ).add( &bq05 );
NahuelM 12:12c162dc8893 123 notch_50_1.add( &bq13 ).add( &bq14 ).add( &bq15 );
NahuelM 12:12c162dc8893 124 notch_50_2.add( &bq23 ).add( &bq24 ).add( &bq25 );
NahuelM 12:12c162dc8893 125 high_pass_0.add( &bq06 ).add( &bq07 );
NahuelM 12:12c162dc8893 126 high_pass_1.add( &bq16 ).add( &bq17 );
NahuelM 12:12c162dc8893 127 high_pass_2.add( &bq26 ).add( &bq27 );
NahuelM 12:12c162dc8893 128 low_pass_0.add( &bq9 ).add( &bq10 ).add( &bq11 );
NahuelM 12:12c162dc8893 129 low_pass_1.add( &bq19 ).add( &bq110 ).add( &bq111 );
NahuelM 12:12c162dc8893 130 low_pass_2.add( &bq29 ).add( &bq210 ).add( &bq211 );
NahuelM 3:1d43dd4f37eb 131
NahuelM 11:c8b6a2b314c3 132 change_state.attach( &calibrate,1);
NahuelM 11:c8b6a2b314c3 133 change_state2.attach( &run,11);
LeeJon 13:0adbf6a5de37 134 controlstarterTimeout.attach(&start_controlling,11); ///// ook nieuw
pbaardwijk 9:1cb2d5ab51e6 135 emgSampleTicker.attach( &emgSample, 0.005); //200Hz
NahuelM 3:1d43dd4f37eb 136
LeeJon 7:eed677b636d3 137 // treshold = (cali_max-cali_min)*treshold_multiplier;
LeeJon 5:bb77e2a6c1e8 138 // servoTick.attach(&control_servo, 1/Frequency);
NahuelM 4:e59a99c5aa08 139 ServoPWMpin.period(0.01f); // 0.01 second period
NahuelM 4:e59a99c5aa08 140
NahuelM 0:46582bba07d2 141 while (true) {
pbaardwijk 6:6cb7c0247560 142 // pc.printf("\n\r%f", Norm_EMG_0);
NahuelM 3:1d43dd4f37eb 143
NahuelM 3:1d43dd4f37eb 144 if (go_emgSample == true){
NahuelM 3:1d43dd4f37eb 145 EMG_filter();
NahuelM 3:1d43dd4f37eb 146 }
NahuelM 1:83fccd7d8483 147
NahuelM 1:83fccd7d8483 148 if (Controller_Flag == true){
LeeJon 13:0adbf6a5de37 149 if(Norm_EMG_2 > statechange_threshold){
LeeJon 13:0adbf6a5de37 150 binary_norm_emg_2 = 1;
LeeJon 13:0adbf6a5de37 151 }else{
LeeJon 13:0adbf6a5de37 152 binary_norm_emg_2 = 0;
LeeJon 13:0adbf6a5de37 153 }
LeeJon 13:0adbf6a5de37 154 if((binary_norm_emg_2 != binary_norm_emg_2_previous)&&binary_norm_emg_2&&state_resettime_value){
LeeJon 13:0adbf6a5de37 155 change_scoopstate();
LeeJon 13:0adbf6a5de37 156 }
LeeJon 13:0adbf6a5de37 157 binary_norm_emg_2_previous = binary_norm_emg_2;
NahuelM 4:e59a99c5aa08 158
LeeJon 13:0adbf6a5de37 159 switch(scoopstate){
LeeJon 13:0adbf6a5de37 160 case STATE_MOVE:
LeeJon 13:0adbf6a5de37 161 Slide_Controller();
LeeJon 13:0adbf6a5de37 162 LED_STATE_MOVE = 1;
LeeJon 13:0adbf6a5de37 163 LED_STATE_GRAB = 0;
LeeJon 13:0adbf6a5de37 164 break;
LeeJon 13:0adbf6a5de37 165 case STATE_GRAB:
LeeJon 13:0adbf6a5de37 166 Lift_Controller();
NahuelM 14:a27de0ddf09f 167 control_servo(Norm_EMG_1);
LeeJon 13:0adbf6a5de37 168 LED_STATE_GRAB = 1;
LeeJon 13:0adbf6a5de37 169 LED_STATE_MOVE = 0;
LeeJon 13:0adbf6a5de37 170 break;
LeeJon 13:0adbf6a5de37 171 }
LeeJon 13:0adbf6a5de37 172
NahuelM 2:57523bb4e9c6 173 Controller_Flag = false;
NahuelM 1:83fccd7d8483 174 }
NahuelM 3:1d43dd4f37eb 175
NahuelM 0:46582bba07d2 176 }
NahuelM 0:46582bba07d2 177 return 0;
NahuelM 0:46582bba07d2 178 }
LeeJon 5:bb77e2a6c1e8 179
NahuelM 1:83fccd7d8483 180 void Ticker_Flag(){
NahuelM 1:83fccd7d8483 181 Controller_Flag = true;
NahuelM 0:46582bba07d2 182 }
NahuelM 0:46582bba07d2 183
NahuelM 0:46582bba07d2 184 void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
NahuelM 2:57523bb4e9c6 185
NahuelM 2:57523bb4e9c6 186 Slide_Counts = Slide_Encoder.getPulses();
NahuelM 2:57523bb4e9c6 187 Slide_Revolutions = Slide_Counts /(32*131);
NahuelM 2:57523bb4e9c6 188 Slide_Angle = Slide_Revolutions*2*pi;
NahuelM 2:57523bb4e9c6 189 Slide_Position = Slide_Angle*Slide_Radius + 135;
NahuelM 2:57523bb4e9c6 190
NahuelM 12:12c162dc8893 191 Slide_Desired_speed= (-Norm_EMG_1+Norm_EMG_0)*Slide_Multiplier;
NahuelM 11:c8b6a2b314c3 192
NahuelM 11:c8b6a2b314c3 193
NahuelM 1:83fccd7d8483 194 if (Slide_Position < Start_slow && Slide_Desired_speed > 0){
NahuelM 1:83fccd7d8483 195 Slide_Desired_speed *= (Slide_Position-Start_lock)/(Start_slow-Start_lock);
NahuelM 1:83fccd7d8483 196
NahuelM 0:46582bba07d2 197 }
NahuelM 1:83fccd7d8483 198 if (Slide_Position > End_slow && Slide_Desired_speed < 0){
NahuelM 1:83fccd7d8483 199 Slide_Desired_speed *= (End_lock-Slide_Position)/(End_lock-End_slow);
NahuelM 0:46582bba07d2 200 }
NahuelM 11:c8b6a2b314c3 201
NahuelM 1:83fccd7d8483 202 Slide_Prev_delta_speed = Slide_Delta_speed;
NahuelM 1:83fccd7d8483 203 Slide_Delta_speed = Slide_Desired_speed-Slide_Curr_speed; // P
NahuelM 1:83fccd7d8483 204 Slide_Int_delta_speed += Slide_Delta_speed/Frequency; // I
NahuelM 11:c8b6a2b314c3 205 if (Slide_Int_delta_speed > 1){Slide_Int_delta_speed = 1;}
NahuelM 11:c8b6a2b314c3 206 if (Slide_Int_delta_speed < -1){Slide_Int_delta_speed = -1;}
NahuelM 11:c8b6a2b314c3 207 Slide_Int_delta_speed *= 1/1.3;
NahuelM 12:12c162dc8893 208 pc.printf("%f - %f - %f \r\n",Norm_EMG_0, Norm_EMG_1, Slide_Delta_speed);
NahuelM 12:12c162dc8893 209 Slide_PI = k1*Slide_Delta_speed + k2*Slide_Int_delta_speed;
NahuelM 1:83fccd7d8483 210 if (Slide_PI<0){
NahuelM 1:83fccd7d8483 211 SlideMotor_Direction = 0;
NahuelM 1:83fccd7d8483 212 }else{
NahuelM 1:83fccd7d8483 213 SlideMotor_Direction = 1;
NahuelM 1:83fccd7d8483 214 }
NahuelM 1:83fccd7d8483 215
NahuelM 1:83fccd7d8483 216 Motor1_pwm.write(abs(Slide_PI));
NahuelM 1:83fccd7d8483 217 //return k1*Delta_speed + k2*Int_delta_speed;
NahuelM 1:83fccd7d8483 218 }
NahuelM 1:83fccd7d8483 219
NahuelM 12:12c162dc8893 220 void Lift_Timeout_Return(){
NahuelM 12:12c162dc8893 221 Lift_Timeout_Switch = true;
NahuelM 12:12c162dc8893 222 }
NahuelM 12:12c162dc8893 223
NahuelM 1:83fccd7d8483 224 void Lift_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
NahuelM 1:83fccd7d8483 225
NahuelM 2:57523bb4e9c6 226 Lift_Counts = Lift_Encoder.getPulses();
NahuelM 2:57523bb4e9c6 227 Lift_Revolutions = Lift_Counts /(32*131);
NahuelM 2:57523bb4e9c6 228 Lift_Angle = Lift_Revolutions*2*pi;
NahuelM 2:57523bb4e9c6 229 Lift_Position = Lift_Angle*Lift_Radius;
NahuelM 2:57523bb4e9c6 230
NahuelM 12:12c162dc8893 231 if(Norm_EMG_0 > 0.6 && Lift_Timeout_Switch == true){
LeeJon 13:0adbf6a5de37 232 Lift_Switch = !Lift_Switch;
NahuelM 12:12c162dc8893 233 Lift_Timeout_Switch = false;
NahuelM 12:12c162dc8893 234 Lift_Timeout.attach(Lift_Timeout_Return, 1);
NahuelM 12:12c162dc8893 235 }
NahuelM 12:12c162dc8893 236
NahuelM 12:12c162dc8893 237 Lift_Desired_position = Lift_Switch*150;
NahuelM 3:1d43dd4f37eb 238 //pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
NahuelM 1:83fccd7d8483 239 Lift_Prev_delta_position = Lift_Delta_position;
NahuelM 1:83fccd7d8483 240 Lift_Delta_position = Lift_Desired_position-Lift_Position; // P
NahuelM 1:83fccd7d8483 241 Lift_Int_delta_position += Lift_Delta_position/Frequency; // I
NahuelM 1:83fccd7d8483 242
NahuelM 11:c8b6a2b314c3 243 Lift_Deriv_delta_position = (Lift_Delta_position-Lift_Prev_delta_position)*Frequency; //D
NahuelM 11:c8b6a2b314c3 244 if (Lift_Int_delta_position > 1){Lift_Int_delta_position = 1;}
NahuelM 11:c8b6a2b314c3 245 if (Lift_Int_delta_position < -1){Lift_Int_delta_position = -1;}
NahuelM 12:12c162dc8893 246
NahuelM 11:c8b6a2b314c3 247 Lift_PI = Lift_k1*Lift_Delta_position + 0*Lift_Int_delta_position + Lift_k3*Lift_Deriv_delta_position;
NahuelM 1:83fccd7d8483 248 if (Lift_PI<0){
NahuelM 1:83fccd7d8483 249 LiftMotor_Direction = 1;
NahuelM 1:83fccd7d8483 250 }else{
NahuelM 1:83fccd7d8483 251 LiftMotor_Direction = 0;
NahuelM 1:83fccd7d8483 252 }
NahuelM 1:83fccd7d8483 253
NahuelM 1:83fccd7d8483 254 Motor2_pwm.write(abs(Lift_PI));
NahuelM 1:83fccd7d8483 255 //return k1*Delta_speed + k2*Int_delta_speed;
NahuelM 0:46582bba07d2 256 }