Gerber Loman / Mbed 2 deprecated MBR_Script_Group_20

Script of MBR Group 20. Control of robot by EMG and/or potmeters

Dependencies:   Encoder HIDScope MODSERIAL biquadFilter mbed

Fork of Script_Group_20 by Gerber Loman

main.cpp@3:36e706d6b3d2, 2017-10-23 (annotated)

Committer:
paulineoonk
Date:
Mon Oct 23 15:26:44 2017 +0000
Revision:
3:36e706d6b3d2
Parent:
2:bc01b1ce8fb6
Child:
4:5607088ef6f5
Samengesteld emg en control. Faalt!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Miriam 0:d5fb29bc0847 1 //libaries
Miriam 0:d5fb29bc0847 2 #include "mbed.h"
Miriam 0:d5fb29bc0847 3 #include "BiQuad.h"
Miriam 0:d5fb29bc0847 4 #include "HIDScope.h"
Miriam 0:d5fb29bc0847 5 #include "encoder.h"
Miriam 0:d5fb29bc0847 6 #include "MODSERIAL.h"
Miriam 0:d5fb29bc0847 7
paulineoonk 3:36e706d6b3d2 8 //globalvariables Motor
paulineoonk 3:36e706d6b3d2 9 Ticker Treecko; //We make a awesome ticker for our control system
paulineoonk 3:36e706d6b3d2 10 PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed
paulineoonk 3:36e706d6b3d2 11 DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control
Miriam 0:d5fb29bc0847 12
paulineoonk 3:36e706d6b3d2 13 Encoder motor1(D13,D12,true);
paulineoonk 3:36e706d6b3d2 14 MODSERIAL pc(USBTX,USBRX);
paulineoonk 3:36e706d6b3d2 15
paulineoonk 3:36e706d6b3d2 16 double PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
paulineoonk 3:36e706d6b3d2 17 const double Ts = 0.1; // tickettijd/ sample time
paulineoonk 3:36e706d6b3d2 18 double e_prev = 0;
paulineoonk 3:36e706d6b3d2 19 double e_int = 0;
paulineoonk 3:36e706d6b3d2 20
paulineoonk 3:36e706d6b3d2 21 //globalvariables filter
Miriam 0:d5fb29bc0847 22
Miriam 0:d5fb29bc0847 23 //Hidscope aanmaken
Miriam 0:d5fb29bc0847 24 HIDScope scope(2);
paulineoonk 3:36e706d6b3d2 25 double maxi = 0.12; // max signal after filtering, 0.1-0.12
Miriam 0:d5fb29bc0847 26
Miriam 0:d5fb29bc0847 27 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
Miriam 0:d5fb29bc0847 28 BiQuad N1( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
Miriam 0:d5fb29bc0847 29 BiQuadChain NF;
Miriam 0:d5fb29bc0847 30 BiQuad HP1( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
Miriam 0:d5fb29bc0847 31 BiQuad HP2( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
Miriam 0:d5fb29bc0847 32 BiQuadChain HPF;
Miriam 0:d5fb29bc0847 33 BiQuad LP1( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
Miriam 0:d5fb29bc0847 34 BiQuad LP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
Miriam 0:d5fb29bc0847 35 BiQuadChain LPF;
Miriam 0:d5fb29bc0847 36
paulineoonk 3:36e706d6b3d2 37 double f = 500; // frequency
paulineoonk 3:36e706d6b3d2 38 double dt = 1/f; // sample frequency
Miriam 0:d5fb29bc0847 39 AnalogIn emg(A0); // EMG lezen
Miriam 0:d5fb29bc0847 40
paulineoonk 3:36e706d6b3d2 41 double GetReferencePosition()
Miriam 0:d5fb29bc0847 42 {
Miriam 0:d5fb29bc0847 43 double emgNotch = NF.step(emg.read() ); // Notch filter
Miriam 0:d5fb29bc0847 44 double emgHP = HPF.step(emgNotch); // High-pass filter: also normalises around 0.
Miriam 0:d5fb29bc0847 45 double emgAbsHP = abs(emgHP); // Take absolute value
Miriam 0:d5fb29bc0847 46 double emgLP = LPF.step(emgAbsHP); // Low-pass filter: creates envelope
Miriam 0:d5fb29bc0847 47 double emgMax = maxi; //(emgLP); // moet waarde 'schatten' voor de max, want je leest de data live. voorbeeld: 0.1, maar mogelijk 0.2 kiezen voor veiligheidsfactor. Dan gaat motor alleen maximaal op 1/2 vermogen.
Miriam 0:d5fb29bc0847 48 double emgFiltered = emgLP/emgMax; // Scale to maximum signal: useful for motor
Miriam 0:d5fb29bc0847 49 if (emgFiltered >1)
Miriam 0:d5fb29bc0847 50 {
Miriam 0:d5fb29bc0847 51 emgFiltered=1.00;
Miriam 0:d5fb29bc0847 52 }
Miriam 0:d5fb29bc0847 53 scope.set(0,emgFiltered);
Miriam 0:d5fb29bc0847 54 scope.set(1,emg.read());
Miriam 0:d5fb29bc0847 55 scope.send();
paulineoonk 3:36e706d6b3d2 56 //pcbaud moet erbij
paulineoonk 3:36e706d6b3d2 57 printf("emgFiltered = %d", emgFiltered);
paulineoonk 3:36e706d6b3d2 58 int maxwaarde = 4096; // = 64x64
paulineoonk 3:36e706d6b3d2 59 double refP = emgFiltered*maxwaarde;
Miriam 2:bc01b1ce8fb6 60 return refP; // value between 0 and 4096
Miriam 0:d5fb29bc0847 61 }
paulineoonk 3:36e706d6b3d2 62 double Encoder ()
paulineoonk 3:36e706d6b3d2 63 {
paulineoonk 3:36e706d6b3d2 64 double Huidigepositie = motor1.getPosition ();
paulineoonk 3:36e706d6b3d2 65 return Huidigepositie; // huidige positie = current position
paulineoonk 3:36e706d6b3d2 66 }
paulineoonk 3:36e706d6b3d2 67 double FeedBackControl(double error, double &e_prev, double &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
Miriam 0:d5fb29bc0847 68 {
paulineoonk 3:36e706d6b3d2 69 double kp = 0.001; // has jet to be scaled
paulineoonk 3:36e706d6b3d2 70 double Proportional= kp*error;
Miriam 0:d5fb29bc0847 71
paulineoonk 3:36e706d6b3d2 72 double kd = 0.0004; // has jet to be scaled
paulineoonk 3:36e706d6b3d2 73 double VelocityError = (error - e_prev)/Ts;
paulineoonk 3:36e706d6b3d2 74 double Derivative = kd*VelocityError;
Miriam 0:d5fb29bc0847 75 e_prev = error;
Miriam 0:d5fb29bc0847 76
paulineoonk 3:36e706d6b3d2 77 double ki = 0.00005; // has jet to be scaled
Miriam 0:d5fb29bc0847 78 e_int = e_int+Ts*error;
paulineoonk 3:36e706d6b3d2 79 double Integrator = ki*e_int;
Miriam 0:d5fb29bc0847 80
Miriam 0:d5fb29bc0847 81
paulineoonk 3:36e706d6b3d2 82 double motorValue = Proportional + Integrator + Derivative;
Miriam 0:d5fb29bc0847 83 return motorValue;
Miriam 0:d5fb29bc0847 84 }
Miriam 0:d5fb29bc0847 85
paulineoonk 3:36e706d6b3d2 86 void SetMotor1(double motorValue)
Miriam 0:d5fb29bc0847 87 {
Miriam 0:d5fb29bc0847 88 if (motorValue >= 0)
Miriam 0:d5fb29bc0847 89 {
Miriam 0:d5fb29bc0847 90 M1D = 0;
Miriam 0:d5fb29bc0847 91 }
Miriam 0:d5fb29bc0847 92 else
Miriam 0:d5fb29bc0847 93 {
Miriam 0:d5fb29bc0847 94 M1D = 1;
Miriam 0:d5fb29bc0847 95 }
Miriam 0:d5fb29bc0847 96
Miriam 0:d5fb29bc0847 97 if (fabs(motorValue) > 1)
Miriam 0:d5fb29bc0847 98 {
Miriam 0:d5fb29bc0847 99 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
Miriam 0:d5fb29bc0847 100 }
Miriam 0:d5fb29bc0847 101 else
Miriam 0:d5fb29bc0847 102 {
Miriam 0:d5fb29bc0847 103 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
Miriam 0:d5fb29bc0847 104 }
Miriam 0:d5fb29bc0847 105 }
paulineoonk 3:36e706d6b3d2 106 void MeasureAndControl ()
Miriam 0:d5fb29bc0847 107 {
paulineoonk 3:36e706d6b3d2 108 // hier the control of the control system
paulineoonk 3:36e706d6b3d2 109 double refP = GetReferencePosition();
paulineoonk 3:36e706d6b3d2 110 double Huidigepositie = Encoder();
paulineoonk 3:36e706d6b3d2 111 double error = (refP - Huidigepositie);// make an error
paulineoonk 3:36e706d6b3d2 112 double motorValue = FeedBackControl(error, e_prev, e_int);
Miriam 0:d5fb29bc0847 113 SetMotor1(motorValue);
Miriam 0:d5fb29bc0847 114 }
Miriam 0:d5fb29bc0847 115
Miriam 0:d5fb29bc0847 116
Miriam 0:d5fb29bc0847 117 int main()
Miriam 0:d5fb29bc0847 118 {
Miriam 0:d5fb29bc0847 119 NF.add( &N1 );
Miriam 0:d5fb29bc0847 120 HPF.add( &HP1 ).add( &HP2 );
Miriam 0:d5fb29bc0847 121 LPF.add( &LP1 ).add( &LP2 );
paulineoonk 3:36e706d6b3d2 122
paulineoonk 3:36e706d6b3d2 123 Treecko.attach(MeasureAndControl, 0.1); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
paulineoonk 3:36e706d6b3d2 124 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
Miriam 0:d5fb29bc0847 125
paulineoonk 3:36e706d6b3d2 126 //emgverwerkticker.attach(&emgverwerk,dt);
paulineoonk 3:36e706d6b3d2 127 //HIDinticker.attach(&ReadFilteredSignal(emgFiltered), 0.01);
paulineoonk 3:36e706d6b3d2 128 while(true)
Miriam 0:d5fb29bc0847 129 {
paulineoonk 3:36e706d6b3d2 130 }
paulineoonk 3:36e706d6b3d2 131
paulineoonk 3:36e706d6b3d2 132
paulineoonk 3:36e706d6b3d2 133 }