read emg to turn motor shaft
Dependencies: MODSERIAL QEI mbed
main.cpp@0:e46276179d7f, 2018-10-01 (annotated)
- Committer:
- MaikOvermars
- Date:
- Mon Oct 01 13:40:37 2018 +0000
- Revision:
- 0:e46276179d7f
test emg to turn motor shaft
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MaikOvermars | 0:e46276179d7f | 1 | #include "mbed.h" |
MaikOvermars | 0:e46276179d7f | 2 | #include "MODSERIAL.h" |
MaikOvermars | 0:e46276179d7f | 3 | #include "QEI.h" |
MaikOvermars | 0:e46276179d7f | 4 | |
MaikOvermars | 0:e46276179d7f | 5 | PwmOut pwmpin(D5); |
MaikOvermars | 0:e46276179d7f | 6 | DigitalOut directionpin(D4); |
MaikOvermars | 0:e46276179d7f | 7 | AnalogIn potmeter(A2); |
MaikOvermars | 0:e46276179d7f | 8 | |
MaikOvermars | 0:e46276179d7f | 9 | AnalogIn emg0( A0 ); |
MaikOvermars | 0:e46276179d7f | 10 | AnalogIn emg1( A1 ); |
MaikOvermars | 0:e46276179d7f | 11 | |
MaikOvermars | 0:e46276179d7f | 12 | MODSERIAL pc(USBTX, USBRX); |
MaikOvermars | 0:e46276179d7f | 13 | QEI wheel (D13, D12, NC, 32); |
MaikOvermars | 0:e46276179d7f | 14 | |
MaikOvermars | 0:e46276179d7f | 15 | float angle_resolution = (360.0/32.0)*(1.0/131.0); //degrees/counts |
MaikOvermars | 0:e46276179d7f | 16 | |
MaikOvermars | 0:e46276179d7f | 17 | float mean(float *samples, int n) { |
MaikOvermars | 0:e46276179d7f | 18 | float sum = 0.0; |
MaikOvermars | 0:e46276179d7f | 19 | for (int i=0; i<n; i++) { |
MaikOvermars | 0:e46276179d7f | 20 | sum += samples[i]; |
MaikOvermars | 0:e46276179d7f | 21 | } |
MaikOvermars | 0:e46276179d7f | 22 | return sum / (float)n; |
MaikOvermars | 0:e46276179d7f | 23 | } |
MaikOvermars | 0:e46276179d7f | 24 | |
MaikOvermars | 0:e46276179d7f | 25 | int main() |
MaikOvermars | 0:e46276179d7f | 26 | { |
MaikOvermars | 0:e46276179d7f | 27 | pc.baud(115200); |
MaikOvermars | 0:e46276179d7f | 28 | |
MaikOvermars | 0:e46276179d7f | 29 | pwmpin.period_us(60); |
MaikOvermars | 0:e46276179d7f | 30 | int pulses = 0; |
MaikOvermars | 0:e46276179d7f | 31 | float angle; |
MaikOvermars | 0:e46276179d7f | 32 | int n = 1; |
MaikOvermars | 0:e46276179d7f | 33 | int i = 0; |
MaikOvermars | 0:e46276179d7f | 34 | float meanmuscle; |
MaikOvermars | 0:e46276179d7f | 35 | float u[500]; |
MaikOvermars | 0:e46276179d7f | 36 | while (true) { |
MaikOvermars | 0:e46276179d7f | 37 | u[i] = fabs(2*(emg0.read() - 0.5)); |
MaikOvermars | 0:e46276179d7f | 38 | |
MaikOvermars | 0:e46276179d7f | 39 | pulses = wheel.getPulses(); |
MaikOvermars | 0:e46276179d7f | 40 | angle = pulses*angle_resolution; |
MaikOvermars | 0:e46276179d7f | 41 | if(n%500 == 0){ |
MaikOvermars | 0:e46276179d7f | 42 | pc.printf("Angle is: %f degrees \r\n", meanmuscle); |
MaikOvermars | 0:e46276179d7f | 43 | meanmuscle = (mean(u,500)-0.1)*4; |
MaikOvermars | 0:e46276179d7f | 44 | directionpin = meanmuscle > 0.0f; |
MaikOvermars | 0:e46276179d7f | 45 | pwmpin = fabs(meanmuscle); |
MaikOvermars | 0:e46276179d7f | 46 | i=0; |
MaikOvermars | 0:e46276179d7f | 47 | } |
MaikOvermars | 0:e46276179d7f | 48 | n++; |
MaikOvermars | 0:e46276179d7f | 49 | i++; |
MaikOvermars | 0:e46276179d7f | 50 | wait(0.001); |
MaikOvermars | 0:e46276179d7f | 51 | } |
MaikOvermars | 0:e46276179d7f | 52 | } |