Nahuel Manterola / Mbed 2 deprecated EMG_Controller_5

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 by Pascal van Baardwijk

main.cpp@4:e59a99c5aa08, 2016-10-25 (annotated)

Committer:
NahuelM
Date:
Tue Oct 25 10:36:50 2016 +0000
Revision:
4:e59a99c5aa08
Parent:
3:1d43dd4f37eb
Child:
5:bb77e2a6c1e8
Servo controller ge?mplementeert;

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 4:e59a99c5aa08 17 //Serial pc(USBTX,USBRX);
NahuelM 0:46582bba07d2 18 Ticker Controller;
NahuelM 0:46582bba07d2 19
NahuelM 4:e59a99c5aa08 20 //char Key;
NahuelM 1:83fccd7d8483 21 bool Controller_Flag=0;
NahuelM 4:e59a99c5aa08 22 //float Frequency = 30;
NahuelM 0:46582bba07d2 23 float Frequency_PWM = 10000;
NahuelM 1:83fccd7d8483 24
NahuelM 1:83fccd7d8483 25 float Slide_Radius = 12.5;
NahuelM 1:83fccd7d8483 26 float Slide_Multiplier = 1;
NahuelM 1:83fccd7d8483 27 float k1 = 1;
NahuelM 1:83fccd7d8483 28 float k2 = 0.02f;
NahuelM 1:83fccd7d8483 29 float k3 = 0.1f;
NahuelM 1:83fccd7d8483 30 float Start_slow = 40;
NahuelM 1:83fccd7d8483 31 float Start_lock = 0;
NahuelM 1:83fccd7d8483 32 float End_slow = 340;
NahuelM 1:83fccd7d8483 33 float End_lock = 380;
NahuelM 1:83fccd7d8483 34 float Slide_Counts;
NahuelM 1:83fccd7d8483 35 float Slide_Revolutions;
NahuelM 1:83fccd7d8483 36 float Slide_Angle;
NahuelM 1:83fccd7d8483 37 float Slide_Position;
NahuelM 0:46582bba07d2 38
NahuelM 1:83fccd7d8483 39 float Slide_Input_force = 0;
NahuelM 1:83fccd7d8483 40 float Slide_Curr_speed = 0;
NahuelM 1:83fccd7d8483 41 float Slide_Desired_speed;
NahuelM 1:83fccd7d8483 42 float Slide_Delta_speed;
NahuelM 1:83fccd7d8483 43 float Slide_Int_delta_speed;
NahuelM 1:83fccd7d8483 44 float Slide_Deriv_delta_speed = 0;
NahuelM 1:83fccd7d8483 45 float Slide_Prev_delta_speed = 0;
NahuelM 1:83fccd7d8483 46 float Slide_PI;
NahuelM 1:83fccd7d8483 47
NahuelM 1:83fccd7d8483 48 float Lift_Radius = 10;
NahuelM 1:83fccd7d8483 49 float Lift_Multiplier = 1;
NahuelM 1:83fccd7d8483 50 float Lift_k1 = 0.08;
NahuelM 1:83fccd7d8483 51 float Lift_k2 = 0.05;
NahuelM 1:83fccd7d8483 52 float Lift_k3 = 0.01;
NahuelM 1:83fccd7d8483 53 float Lift_Start = 0;
NahuelM 1:83fccd7d8483 54 float Lift_End = 50;
NahuelM 1:83fccd7d8483 55 float Lift_Counts;
NahuelM 1:83fccd7d8483 56 float Lift_Revolutions;
NahuelM 1:83fccd7d8483 57 float Lift_Angle;
NahuelM 1:83fccd7d8483 58 float Lift_Position;
NahuelM 1:83fccd7d8483 59
NahuelM 1:83fccd7d8483 60 float Lift_Input_force = 0;
NahuelM 1:83fccd7d8483 61 float Lift_Desired_position;
NahuelM 1:83fccd7d8483 62 float Lift_Delta_position;
NahuelM 1:83fccd7d8483 63 float Lift_Int_delta_position;
NahuelM 1:83fccd7d8483 64 float Lift_Deriv_delta_position = 0;
NahuelM 1:83fccd7d8483 65 float Lift_Prev_delta_position = 0;
NahuelM 1:83fccd7d8483 66 float Lift_PI;
NahuelM 0:46582bba07d2 67
NahuelM 0:46582bba07d2 68 void Slide_Controller();
NahuelM 1:83fccd7d8483 69 void Lift_Controller();
NahuelM 1:83fccd7d8483 70 void Ticker_Flag();
NahuelM 0:46582bba07d2 71
NahuelM 0:46582bba07d2 72 int main()
NahuelM 0:46582bba07d2 73 {
NahuelM 0:46582bba07d2 74 Motor1_pwm.period(1.0/Frequency_PWM);//T=1/f
NahuelM 1:83fccd7d8483 75 Motor2_pwm.period(1.0/Frequency_PWM);//T=1/f
NahuelM 1:83fccd7d8483 76 Controller.attach(&Ticker_Flag,1/Frequency);
NahuelM 0:46582bba07d2 77 pc.baud(9600);
NahuelM 2:57523bb4e9c6 78
NahuelM 3:1d43dd4f37eb 79
NahuelM 3:1d43dd4f37eb 80 Lift_Input_force = Potmeter.read();
NahuelM 3:1d43dd4f37eb 81 Slide_Input_force = Potmeter2.read();
NahuelM 3:1d43dd4f37eb 82
NahuelM 3:1d43dd4f37eb 83 notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 );
NahuelM 3:1d43dd4f37eb 84 high_pass.add( &bq4 ).add( &bq5 );
NahuelM 3:1d43dd4f37eb 85
NahuelM 3:1d43dd4f37eb 86 change_state.attach( &calibrate,5);
NahuelM 3:1d43dd4f37eb 87 change_state2.attach( &run,10);
NahuelM 3:1d43dd4f37eb 88 emgSampleTicker.attach( &emgSample, 0.002);
NahuelM 3:1d43dd4f37eb 89
NahuelM 4:e59a99c5aa08 90 treshold = (cali_max-cali_min)*treshold_multiplier;
NahuelM 4:e59a99c5aa08 91 servoTick.attach(&control_servo, 1/Frequency);
NahuelM 4:e59a99c5aa08 92 ServoPWMpin.period(0.01f); // 0.01 second period
NahuelM 4:e59a99c5aa08 93
NahuelM 0:46582bba07d2 94 while (true) {
NahuelM 3:1d43dd4f37eb 95 pc.printf("\n\r%f", Norm_EMG_0);
NahuelM 3:1d43dd4f37eb 96
NahuelM 3:1d43dd4f37eb 97 if (go_emgSample == true){
NahuelM 3:1d43dd4f37eb 98 EMG_filter();
NahuelM 3:1d43dd4f37eb 99 }
NahuelM 1:83fccd7d8483 100
NahuelM 1:83fccd7d8483 101 if (Controller_Flag == true){
NahuelM 1:83fccd7d8483 102 Slide_Controller();
NahuelM 1:83fccd7d8483 103 Lift_Controller();
NahuelM 4:e59a99c5aa08 104 control_servo();
NahuelM 4:e59a99c5aa08 105
NahuelM 2:57523bb4e9c6 106 Controller_Flag = false;
NahuelM 1:83fccd7d8483 107 }
NahuelM 1:83fccd7d8483 108
NahuelM 3:1d43dd4f37eb 109
NahuelM 2:57523bb4e9c6 110
NahuelM 2:57523bb4e9c6 111
NahuelM 0:46582bba07d2 112
NahuelM 0:46582bba07d2 113 }
NahuelM 0:46582bba07d2 114 return 0;
NahuelM 0:46582bba07d2 115 }
NahuelM 1:83fccd7d8483 116 void Ticker_Flag(){
NahuelM 1:83fccd7d8483 117 Controller_Flag = true;
NahuelM 0:46582bba07d2 118 }
NahuelM 0:46582bba07d2 119
NahuelM 0:46582bba07d2 120 void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
NahuelM 2:57523bb4e9c6 121
NahuelM 2:57523bb4e9c6 122 Slide_Counts = Slide_Encoder.getPulses();
NahuelM 2:57523bb4e9c6 123 Slide_Revolutions = Slide_Counts /(32*131);
NahuelM 2:57523bb4e9c6 124 Slide_Angle = Slide_Revolutions*2*pi;
NahuelM 2:57523bb4e9c6 125 Slide_Position = Slide_Angle*Slide_Radius + 135;
NahuelM 2:57523bb4e9c6 126
NahuelM 3:1d43dd4f37eb 127 Slide_Desired_speed= (Norm_EMG_0*2 -1)*Slide_Multiplier;
NahuelM 1:83fccd7d8483 128
NahuelM 1:83fccd7d8483 129 if (Slide_Position < Start_slow && Slide_Desired_speed > 0){
NahuelM 1:83fccd7d8483 130 Slide_Desired_speed *= (Slide_Position-Start_lock)/(Start_slow-Start_lock);
NahuelM 1:83fccd7d8483 131
NahuelM 0:46582bba07d2 132 }
NahuelM 1:83fccd7d8483 133 if (Slide_Position > End_slow && Slide_Desired_speed < 0){
NahuelM 1:83fccd7d8483 134 Slide_Desired_speed *= (End_lock-Slide_Position)/(End_lock-End_slow);
NahuelM 0:46582bba07d2 135 }
NahuelM 1:83fccd7d8483 136 Slide_Prev_delta_speed = Slide_Delta_speed;
NahuelM 1:83fccd7d8483 137 Slide_Delta_speed = Slide_Desired_speed-Slide_Curr_speed; // P
NahuelM 1:83fccd7d8483 138 Slide_Int_delta_speed += Slide_Delta_speed/Frequency; // I
NahuelM 1:83fccd7d8483 139
NahuelM 1:83fccd7d8483 140 Slide_PI = k1*Slide_Delta_speed + k2*Slide_Int_delta_speed;
NahuelM 1:83fccd7d8483 141 if (Slide_PI<0){
NahuelM 1:83fccd7d8483 142 SlideMotor_Direction = 0;
NahuelM 1:83fccd7d8483 143 }else{
NahuelM 1:83fccd7d8483 144 SlideMotor_Direction = 1;
NahuelM 1:83fccd7d8483 145 }
NahuelM 1:83fccd7d8483 146
NahuelM 1:83fccd7d8483 147 Motor1_pwm.write(abs(Slide_PI));
NahuelM 1:83fccd7d8483 148 //return k1*Delta_speed + k2*Int_delta_speed;
NahuelM 1:83fccd7d8483 149 }
NahuelM 1:83fccd7d8483 150
NahuelM 1:83fccd7d8483 151 void Lift_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
NahuelM 1:83fccd7d8483 152
NahuelM 2:57523bb4e9c6 153 Lift_Counts = Lift_Encoder.getPulses();
NahuelM 2:57523bb4e9c6 154 Lift_Revolutions = Lift_Counts /(32*131);
NahuelM 2:57523bb4e9c6 155 Lift_Angle = Lift_Revolutions*2*pi;
NahuelM 2:57523bb4e9c6 156 Lift_Position = Lift_Angle*Lift_Radius;
NahuelM 2:57523bb4e9c6 157
NahuelM 2:57523bb4e9c6 158 Lift_Desired_position = (Lift_Input_force*2-1)*30*Lift_Multiplier;
NahuelM 3:1d43dd4f37eb 159 //pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
NahuelM 1:83fccd7d8483 160 Lift_Prev_delta_position = Lift_Delta_position;
NahuelM 1:83fccd7d8483 161 Lift_Delta_position = Lift_Desired_position-Lift_Position; // P
NahuelM 1:83fccd7d8483 162 Lift_Int_delta_position += Lift_Delta_position/Frequency; // I
NahuelM 1:83fccd7d8483 163 Lift_Deriv_delta_position = (Lift_Delta_position-Lift_Prev_delta_position)*Frequency;
NahuelM 1:83fccd7d8483 164
NahuelM 1:83fccd7d8483 165 Lift_PI = Lift_k1*Lift_Delta_position + Lift_k2*Lift_Int_delta_position + Lift_k3*Lift_Deriv_delta_position;
NahuelM 1:83fccd7d8483 166 if (Lift_PI<0){
NahuelM 1:83fccd7d8483 167 LiftMotor_Direction = 1;
NahuelM 1:83fccd7d8483 168 }else{
NahuelM 1:83fccd7d8483 169 LiftMotor_Direction = 0;
NahuelM 1:83fccd7d8483 170 }
NahuelM 1:83fccd7d8483 171
NahuelM 1:83fccd7d8483 172 Motor2_pwm.write(abs(Lift_PI));
NahuelM 1:83fccd7d8483 173 //return k1*Delta_speed + k2*Int_delta_speed;
NahuelM 0:46582bba07d2 174 }