Matthew Maat / Mbed 2 deprecated template_biorobotics

fancy lampje

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@14:dc89250ebc52, 2019-10-14 (annotated)

Committer:
MatthewMaat
Date:
Mon Oct 14 13:42:38 2019 +0000
Revision:
14:dc89250ebc52
Parent:
13:ec4708dab45d
Child:
15:c4799ad02cdc
state machine-not working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
MatthewMaat 13:ec4708dab45d 2 #include "HIDScope.h"
MatthewMaat 11:de4a85703169 3 //#include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
RobertoO 0:67c50348f842 5 //#include "BiQuad.h"
MatthewMaat 8:ec3c634390c7 6 #include "FastPWM.h"
MatthewMaat 2:626688c21b6f 7 #include <iostream>
MatthewMaat 5:cee5f898b350 8 MODSERIAL pc(USBTX, USBRX);
MatthewMaat 13:ec4708dab45d 9 AnalogIn ain2(A2);
MatthewMaat 13:ec4708dab45d 10 AnalogIn ain1(A3);
MatthewMaat 13:ec4708dab45d 11 DigitalOut dir2(D4);
MatthewMaat 13:ec4708dab45d 12 DigitalOut dir1(D7);
MatthewMaat 14:dc89250ebc52 13
MatthewMaat 11:de4a85703169 14
MatthewMaat 11:de4a85703169 15
MatthewMaat 12:7f280a661e71 16 Ticker ticktick;
MatthewMaat 13:ec4708dab45d 17 //D4,D7 direction of motors 2,1 on board, D5,D6- PWM of motors 2,1 on board
MatthewMaat 13:ec4708dab45d 18 PwmOut motor2_pwm(D5);
MatthewMaat 13:ec4708dab45d 19 PwmOut motor1_pwm(D6);
MatthewMaat 13:ec4708dab45d 20 AnalogIn emg0( A0 );
MatthewMaat 13:ec4708dab45d 21 AnalogIn emg1( A1 );
MatthewMaat 13:ec4708dab45d 22
MatthewMaat 13:ec4708dab45d 23 Ticker sample_timer;
MatthewMaat 13:ec4708dab45d 24 HIDScope scope( 2 );
MatthewMaat 14:dc89250ebc52 25 DigitalOut ledred(LED_RED);
MatthewMaat 14:dc89250ebc52 26 DigitalOut ledblue(LED_BLUE);
MatthewMaat 14:dc89250ebc52 27 DigitalOut ledgreen(LED_GREEN);
MatthewMaat 13:ec4708dab45d 28 volatile float P;
MatthewMaat 14:dc89250ebc52 29 enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
MatthewMaat 14:dc89250ebc52 30 states currentState=Operating;
MatthewMaat 14:dc89250ebc52 31 InterruptIn err(SW2);
MatthewMaat 10:e1eb73e19540 32
MatthewMaat 14:dc89250ebc52 33
MatthewMaat 14:dc89250ebc52 34 void read_emg()
MatthewMaat 13:ec4708dab45d 35 {
MatthewMaat 13:ec4708dab45d 36 static int count=0;
MatthewMaat 13:ec4708dab45d 37 static float RMS_value=0;
MatthewMaat 13:ec4708dab45d 38 static float HighPass_value=0;
MatthewMaat 13:ec4708dab45d 39 count+=1;
MatthewMaat 13:ec4708dab45d 40 static float RMS[150];
MatthewMaat 13:ec4708dab45d 41 static float HighPass[30];
MatthewMaat 13:ec4708dab45d 42 float I1;
MatthewMaat 13:ec4708dab45d 43 float If;
MatthewMaat 13:ec4708dab45d 44 I1=emg0.read(); //read signal
MatthewMaat 13:ec4708dab45d 45 HighPass_value+=(I1-HighPass[count%30])/30.0;
MatthewMaat 13:ec4708dab45d 46 HighPass[count%30]=I1;
MatthewMaat 13:ec4708dab45d 47 If=pow(I1-HighPass_value,2.0f); // Highpass-filtered value squared
MatthewMaat 13:ec4708dab45d 48 RMS_value+=(If-RMS[count%150])/150.0;
MatthewMaat 13:ec4708dab45d 49 RMS[count%150]=If;
MatthewMaat 13:ec4708dab45d 50 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MatthewMaat 13:ec4708dab45d 51 P=sqrt(RMS_value);
MatthewMaat 13:ec4708dab45d 52 scope.set(0, P ); // send root mean squared
MatthewMaat 13:ec4708dab45d 53 scope.set(1, emg0.read() );
MatthewMaat 13:ec4708dab45d 54 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
MatthewMaat 13:ec4708dab45d 55 * Ensure that enough channels are available (HIDScope scope( 2 ))
MatthewMaat 13:ec4708dab45d 56 * Finally, send all channels to the PC at once */
MatthewMaat 13:ec4708dab45d 57 scope.send();
MatthewMaat 13:ec4708dab45d 58 /* To indicate that the function is working, the LED is toggled */
MatthewMaat 14:dc89250ebc52 59 ledred=1;
MatthewMaat 14:dc89250ebc52 60 ledgreen=0;
MatthewMaat 14:dc89250ebc52 61 ledblue=1;
MatthewMaat 13:ec4708dab45d 62 }
MatthewMaat 14:dc89250ebc52 63
MatthewMaat 14:dc89250ebc52 64 void set_PWM(void)
MatthewMaat 11:de4a85703169 65 {
MatthewMaat 13:ec4708dab45d 66 float Q;
MatthewMaat 13:ec4708dab45d 67 if (7*P>1)
MatthewMaat 13:ec4708dab45d 68 {
MatthewMaat 13:ec4708dab45d 69 Q=1.0;
MatthewMaat 13:ec4708dab45d 70 }
MatthewMaat 13:ec4708dab45d 71 else
MatthewMaat 13:ec4708dab45d 72 {
MatthewMaat 13:ec4708dab45d 73 Q=7*P;
MatthewMaat 13:ec4708dab45d 74 }
MatthewMaat 13:ec4708dab45d 75 motor1_pwm.write(Q);
MatthewMaat 13:ec4708dab45d 76 motor2_pwm.write(ain1.read());
MatthewMaat 11:de4a85703169 77 }
MatthewMaat 11:de4a85703169 78
MatthewMaat 14:dc89250ebc52 79 void sample()
MatthewMaat 14:dc89250ebc52 80 {
MatthewMaat 14:dc89250ebc52 81 switch(currentState)
MatthewMaat 14:dc89250ebc52 82 {
MatthewMaat 14:dc89250ebc52 83 case Operating:
MatthewMaat 14:dc89250ebc52 84 read_emg();
MatthewMaat 14:dc89250ebc52 85 set_PWM();
MatthewMaat 14:dc89250ebc52 86 break;
MatthewMaat 14:dc89250ebc52 87 case Failure:
MatthewMaat 14:dc89250ebc52 88 ledred=0;
MatthewMaat 14:dc89250ebc52 89 ledgreen=1;
MatthewMaat 14:dc89250ebc52 90 ledblue=1;
MatthewMaat 14:dc89250ebc52 91 break;
MatthewMaat 14:dc89250ebc52 92 default:
MatthewMaat 14:dc89250ebc52 93 ledred=1;
MatthewMaat 14:dc89250ebc52 94 ledgreen=1;
MatthewMaat 14:dc89250ebc52 95 ledblue=0;
MatthewMaat 14:dc89250ebc52 96 break;
MatthewMaat 14:dc89250ebc52 97 }
MatthewMaat 14:dc89250ebc52 98 }
MatthewMaat 14:dc89250ebc52 99
MatthewMaat 14:dc89250ebc52 100 void error_occur()
MatthewMaat 14:dc89250ebc52 101 {
MatthewMaat 14:dc89250ebc52 102 currentState=Failure;
MatthewMaat 14:dc89250ebc52 103 }
MatthewMaat 14:dc89250ebc52 104
MatthewMaat 8:ec3c634390c7 105 int main()
MatthewMaat 4:f988679bf9a1 106 {
MatthewMaat 14:dc89250ebc52 107 pc.baud(115200);
MatthewMaat 14:dc89250ebc52 108 pc.printf("Starting...");
MatthewMaat 14:dc89250ebc52 109 ledred=0;
MatthewMaat 13:ec4708dab45d 110 sample_timer.attach(&sample, 0.002);
MatthewMaat 14:dc89250ebc52 111 err.fall(error_occur);
MatthewMaat 12:7f280a661e71 112 int frequency_pwm=10000;
MatthewMaat 12:7f280a661e71 113 motor1_pwm.period(1.0/frequency_pwm);
MatthewMaat 13:ec4708dab45d 114 motor2_pwm.period(1.0/frequency_pwm);
MatthewMaat 11:de4a85703169 115
MatthewMaat 8:ec3c634390c7 116 while (true) {
MatthewMaat 12:7f280a661e71 117 wait(10);
MatthewMaat 13:ec4708dab45d 118 dir1=!dir1;
MatthewMaat 13:ec4708dab45d 119 dir2=!dir2;
MatthewMaat 4:f988679bf9a1 120 }
MatthewMaat 4:f988679bf9a1 121 }