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@0:46582bba07d2, 2016-10-17 (annotated)

Committer:
NahuelM
Date:
Mon Oct 17 13:53:44 2016 +0000
Revision:
0:46582bba07d2
Child:
1:83fccd7d8483
bericht

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 0:46582bba07d2 3
NahuelM 0:46582bba07d2 4 PwmOut Motor1_pwm(D5);
NahuelM 0:46582bba07d2 5 QEI Encoder(D12,D13,NC,64);
NahuelM 0:46582bba07d2 6 Serial pc(USBTX,USBRX);
NahuelM 0:46582bba07d2 7 Ticker Controller;
NahuelM 0:46582bba07d2 8
NahuelM 0:46582bba07d2 9 float Radius = 10;
NahuelM 0:46582bba07d2 10 float Frequency = 30;
NahuelM 0:46582bba07d2 11 float Multiplier = 1;
NahuelM 0:46582bba07d2 12 float k1 = 1;
NahuelM 0:46582bba07d2 13 float k2 = 0.1f;
NahuelM 0:46582bba07d2 14 float k3 = 0.1f;
NahuelM 0:46582bba07d2 15 float Start_slow = 100;
NahuelM 0:46582bba07d2 16 float Start_lock = 60;
NahuelM 0:46582bba07d2 17 float End_slow = 400;
NahuelM 0:46582bba07d2 18 float End_lock = 440;
NahuelM 0:46582bba07d2 19 float Frequency_PWM = 10000;
NahuelM 0:46582bba07d2 20 float Counts = 0;
NahuelM 0:46582bba07d2 21 float Revolutions = 0;
NahuelM 0:46582bba07d2 22 const double pi = 3.14159265359;
NahuelM 0:46582bba07d2 23
NahuelM 0:46582bba07d2 24 float Input_force = 0.1;
NahuelM 0:46582bba07d2 25 float Angle = 0;
NahuelM 0:46582bba07d2 26 float Curr_speed = 0;
NahuelM 0:46582bba07d2 27 float Desired_speed;
NahuelM 0:46582bba07d2 28 float Delta_speed;
NahuelM 0:46582bba07d2 29 float Int_delta_speed;
NahuelM 0:46582bba07d2 30 float Deriv_delta_speed = 0;
NahuelM 0:46582bba07d2 31 float Prev_delta_speed = 0;
NahuelM 0:46582bba07d2 32
NahuelM 0:46582bba07d2 33 void Slide_Controller();
NahuelM 0:46582bba07d2 34
NahuelM 0:46582bba07d2 35 int main()
NahuelM 0:46582bba07d2 36 {
NahuelM 0:46582bba07d2 37 Motor1_pwm.period(1.0/Frequency_PWM);//T=1/f
NahuelM 0:46582bba07d2 38
NahuelM 0:46582bba07d2 39 Controller.attach(&Slide_Controller,1/Frequency);
NahuelM 0:46582bba07d2 40 pc.baud(9600);
NahuelM 0:46582bba07d2 41 while (true) {
NahuelM 0:46582bba07d2 42 Counts = Encoder.getPulses();
NahuelM 0:46582bba07d2 43 Revolutions = Counts /(32*131);
NahuelM 0:46582bba07d2 44 Angle = Revolutions*2*pi;
NahuelM 0:46582bba07d2 45 char a = pc.getc();
NahuelM 0:46582bba07d2 46 Input_force = (float)a/100;
NahuelM 0:46582bba07d2 47
NahuelM 0:46582bba07d2 48 pc.printf("%f", Input_force);
NahuelM 0:46582bba07d2 49 //Motor1_pwm.write(Slide_Controller(Input_force));
NahuelM 0:46582bba07d2 50
NahuelM 0:46582bba07d2 51 wait(1/Frequency);
NahuelM 0:46582bba07d2 52 }
NahuelM 0:46582bba07d2 53 return 0;
NahuelM 0:46582bba07d2 54 }
NahuelM 0:46582bba07d2 55
NahuelM 0:46582bba07d2 56
NahuelM 0:46582bba07d2 57
NahuelM 0:46582bba07d2 58
NahuelM 0:46582bba07d2 59 float Position(float Angle){
NahuelM 0:46582bba07d2 60 return Angle*Radius;
NahuelM 0:46582bba07d2 61 }
NahuelM 0:46582bba07d2 62
NahuelM 0:46582bba07d2 63 void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
NahuelM 0:46582bba07d2 64
NahuelM 0:46582bba07d2 65 Desired_speed = Input_force*Multiplier;
NahuelM 0:46582bba07d2 66
NahuelM 0:46582bba07d2 67 if (Position(Angle) < Start_slow && Desired_speed < 0){
NahuelM 0:46582bba07d2 68 Desired_speed *= (Position(Angle)-Start_lock)/(Start_slow-Start_lock)*1.5-0.5;
NahuelM 0:46582bba07d2 69 }
NahuelM 0:46582bba07d2 70 if (Position(Angle) > End_slow && Desired_speed > 0){
NahuelM 0:46582bba07d2 71 Desired_speed *= (End_lock-Position(Angle))/(End_lock-End_slow)*1.5-0.5;
NahuelM 0:46582bba07d2 72 }
NahuelM 0:46582bba07d2 73 Prev_delta_speed = Delta_speed;
NahuelM 0:46582bba07d2 74 Delta_speed = Desired_speed-Curr_speed; // P
NahuelM 0:46582bba07d2 75 Int_delta_speed += Delta_speed/Frequency; // I
NahuelM 0:46582bba07d2 76 Deriv_delta_speed = (Delta_speed-Prev_delta_speed)*Frequency; // D
NahuelM 0:46582bba07d2 77 Motor1_pwm.write(k1*Delta_speed + k2*Int_delta_speed + k3*Deriv_delta_speed);
NahuelM 0:46582bba07d2 78 //return k1*Delta_speed + k2*Int_delta_speed + k3*Deriv_delta_speed;
NahuelM 0:46582bba07d2 79 }