groep 16 / Mbed 2 deprecated EMGtomotor

Leipe emg to motor file

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@22:88203d281e98, 2019-10-28 (annotated)

Committer:
lucvandijk
Date:
Mon Oct 28 11:25:04 2019 +0000
Revision:
22:88203d281e98
Parent:
21:db6c27246aa4 
Guus, kiek an!; Die movemotorTo van Jona moet dus veranderd worden, snapte geen conjo van hoe dat werkte. ; Gaat over motor calibratie trouwens;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
sanou8 21:db6c27246aa4 2 #include "MODSERIAL.h"
sanou8 21:db6c27246aa4 3 #include "QEI.h"
sanou8 21:db6c27246aa4 4 #include "FilterDesign.h"
sanou8 21:db6c27246aa4 5 #include "BiQuad.h"
sanou8 21:db6c27246aa4 6 #include "BiQuad4.h"
vsluiter 11:ce72ec658a95 7 #include "HIDScope.h"
vsluiter 0:32bb76391d89 8
sanou8 21:db6c27246aa4 9 QEI Encoder(D12,D13,NC,32);
sanou8 21:db6c27246aa4 10
sanou8 21:db6c27246aa4 11 DigitalOut M1(D4);
sanou8 21:db6c27246aa4 12 DigitalOut M2(D7);
sanou8 21:db6c27246aa4 13
sanou8 21:db6c27246aa4 14
sanou8 21:db6c27246aa4 15 PwmOut E1(D5);
sanou8 21:db6c27246aa4 16 PwmOut E2(D6);
sanou8 21:db6c27246aa4 17
tomlankhorst 19:2bf824669684 18
sanou8 21:db6c27246aa4 19 AnalogIn emg0( A0 ); // EMG input 1
sanou8 21:db6c27246aa4 20 AnalogIn emg1( A1 ); // EMG input 2
sanou8 21:db6c27246aa4 21
sanou8 21:db6c27246aa4 22 float potVal1;
sanou8 21:db6c27246aa4 23 float potVal2;
sanou8 21:db6c27246aa4 24 volatile double emg1_filtered; //measured value of the first emg
sanou8 21:db6c27246aa4 25 volatile double emg2_filtered; //measured value of the second emg
sanou8 21:db6c27246aa4 26 Ticker sample_timer; // Ticker for reading out EMG
tomlankhorst 19:2bf824669684 27 HIDScope scope( 2 );
sanou8 21:db6c27246aa4 28
sanou8 21:db6c27246aa4 29 float pi = 3.14159265359;
vsluiter 2:e314bb3b2d99 30
tomlankhorst 14:f83354387756 31 /** Sample function
tomlankhorst 14:f83354387756 32 * this function samples the emg and sends it to HIDScope
tomlankhorst 14:f83354387756 33 **/
sanou8 21:db6c27246aa4 34
tomlankhorst 14:f83354387756 35 void sample()
vsluiter 2:e314bb3b2d99 36 {
sanou8 21:db6c27246aa4 37 emg1_filtered = emg0.read() ;
sanou8 21:db6c27246aa4 38 emg2_filtered = emg1.read() ;
tomlankhorst 19:2bf824669684 39 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
sanou8 21:db6c27246aa4 40 scope.set(0, emg1_filtered);
sanou8 21:db6c27246aa4 41 scope.set(1, emg2_filtered);
tomlankhorst 19:2bf824669684 42 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
tomlankhorst 19:2bf824669684 43 * Ensure that enough channels are available (HIDScope scope( 2 ))
tomlankhorst 20:97059009a491 44 * Finally, send all channels to the PC at once */
vsluiter 11:ce72ec658a95 45 scope.send();
tomlankhorst 18:21d8e7a81cf5 46 /* To indicate that the function is working, the LED is toggled */
sanou8 21:db6c27246aa4 47 //led = !led;
vsluiter 2:e314bb3b2d99 48 }
vsluiter 0:32bb76391d89 49
sanou8 21:db6c27246aa4 50 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes)
sanou8 21:db6c27246aa4 51 {
sanou8 21:db6c27246aa4 52 float pErrorC;
sanou8 21:db6c27246aa4 53 float pErrorP = 0;
sanou8 21:db6c27246aa4 54 float iError = 0;
sanou8 21:db6c27246aa4 55 float dError;
sanou8 21:db6c27246aa4 56
sanou8 21:db6c27246aa4 57 float Kp = 5;
sanou8 21:db6c27246aa4 58 float Ki = 0.01;
sanou8 21:db6c27246aa4 59 float Kd = 0.7;
sanou8 21:db6c27246aa4 60
sanou8 21:db6c27246aa4 61 float rotC = Enc->getPulses()/(32*131.25);
sanou8 21:db6c27246aa4 62 float rotP = 0;
sanou8 21:db6c27246aa4 63 float MotorPWM;
sanou8 21:db6c27246aa4 64
sanou8 21:db6c27246aa4 65 Timer t;
sanou8 21:db6c27246aa4 66 float tieme = 0;
sanou8 21:db6c27246aa4 67
sanou8 21:db6c27246aa4 68 t.start();
sanou8 21:db6c27246aa4 69 do {
sanou8 21:db6c27246aa4 70 pErrorP = pErrorC;
sanou8 21:db6c27246aa4 71 pErrorC = rotDes - rotC;
sanou8 21:db6c27246aa4 72 iError = iError + pErrorC*tieme;
sanou8 21:db6c27246aa4 73 dError = (pErrorC - pErrorP)/tieme;
sanou8 21:db6c27246aa4 74
sanou8 21:db6c27246aa4 75 MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
sanou8 21:db6c27246aa4 76
sanou8 21:db6c27246aa4 77 if(MotorPWM > 0) {
sanou8 21:db6c27246aa4 78 *M = 0;
sanou8 21:db6c27246aa4 79 *E = MotorPWM;
sanou8 21:db6c27246aa4 80 } else {
sanou8 21:db6c27246aa4 81 *M = 1;
sanou8 21:db6c27246aa4 82 *E = -MotorPWM;
sanou8 21:db6c27246aa4 83 }
sanou8 21:db6c27246aa4 84
sanou8 21:db6c27246aa4 85 rotP = rotC;
sanou8 21:db6c27246aa4 86 rotC = Enc->getPulses()/(32*131.25);
sanou8 21:db6c27246aa4 87 tieme = t.read();
sanou8 21:db6c27246aa4 88 t.reset();
sanou8 21:db6c27246aa4 89 //pc.printf("pError: %f dError: %f iError: %f PWM: %f\n\r", pErrorC, dError, iError, MotorPWM);
sanou8 21:db6c27246aa4 90 } while (pErrorC > 0.01 || pErrorC < -0.01 ||dError > 0.01 || dError < -0.01);
sanou8 21:db6c27246aa4 91 //*E = 0;
sanou8 21:db6c27246aa4 92 t.stop();
sanou8 21:db6c27246aa4 93 }
sanou8 21:db6c27246aa4 94
sanou8 21:db6c27246aa4 95
sanou8 21:db6c27246aa4 96
vsluiter 0:32bb76391d89 97 int main()
tomlankhorst 19:2bf824669684 98 {
tomlankhorst 14:f83354387756 99 /**Attach the 'sample' function to the timer 'sample_timer'.
tomlankhorst 19:2bf824669684 100 * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
vsluiter 4:8b298dfada81 101 */
tomlankhorst 19:2bf824669684 102 sample_timer.attach(&sample, 0.002);
sanou8 21:db6c27246aa4 103 float steps = 100;
tomlankhorst 15:0da764eea774 104
tomlankhorst 14:f83354387756 105 /*empty loop, sample() is executed periodically*/
sanou8 21:db6c27246aa4 106 while (true){
sanou8 21:db6c27246aa4 107 if(emg1_filtered >0.6) {
sanou8 21:db6c27246aa4 108 for(int i = 0; i < steps; i++){
sanou8 21:db6c27246aa4 109 moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
sanou8 21:db6c27246aa4 110 //pc.printf("step: %f\n\r", float(i)/steps);
sanou8 21:db6c27246aa4 111 }
sanou8 21:db6c27246aa4 112 for(int i = steps; i > 0; i--){
sanou8 21:db6c27246aa4 113 moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
sanou8 21:db6c27246aa4 114 }
sanou8 21:db6c27246aa4 115 }
sanou8 21:db6c27246aa4 116 }
vsluiter 0:32bb76391d89 117 }