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@1:83fccd7d8483, 2016-10-24 (annotated)

Committer:
NahuelM
Date:
Mon Oct 24 10:54:55 2016 +0000
Revision:
1:83fccd7d8483
Parent:
0:46582bba07d2
Child:
2:57523bb4e9c6
Controllers

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