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@2:57523bb4e9c6, 2016-10-24 (annotated)

Committer:
NahuelM
Date:
Mon Oct 24 11:31:09 2016 +0000
Revision:
2:57523bb4e9c6
Parent:
1:83fccd7d8483
Child:
3:1d43dd4f37eb
EMG Ingevoegd in controller

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