Thom Kuenen / Mbed 2 deprecated Turning_Motor_V2

Overzetten

Dependencies:   HIDScope MODSERIAL mbed

main.cpp@8:81f2c8ae4427, 2018-10-12 (annotated)

Committer:
ThomBMT
Date:
Fri Oct 12 08:53:12 2018 +0000
Revision:
8:81f2c8ae4427
Parent:
7:d57eba2aed38
Child:
9:836d6ac23481
I am finally able to control the motor using the emg signal obtained from the bicep. This is still very rough and the signal will need to be filtered for this to work proporly but it is a good start;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThomBMT 0:877f950fdfb5 1 #include "mbed.h"
ThomBMT 0:877f950fdfb5 2 #include "MODSERIAL.h"
ThomBMT 0:877f950fdfb5 3 #include "HIDScope.h"
ThomBMT 0:877f950fdfb5 4 MODSERIAL pc(USBTX, USBRX);
ThomBMT 0:877f950fdfb5 5
ThomBMT 0:877f950fdfb5 6 DigitalOut DirectionPin1(D4);
ThomBMT 0:877f950fdfb5 7 PwmOut PwmPin1(D5);
ThomBMT 0:877f950fdfb5 8 DigitalOut DirectionPin2(D7);
ThomBMT 0:877f950fdfb5 9 PwmOut PwmPin2(D6);
ThomBMT 0:877f950fdfb5 10 DigitalIn Knop1(D3);
ThomBMT 0:877f950fdfb5 11 DigitalIn Knop2(D2);
ThomBMT 0:877f950fdfb5 12 DigitalIn Knop3(PTA4);
ThomBMT 0:877f950fdfb5 13 DigitalIn Knop4(PTC6);
ThomBMT 6:bbef54156775 14 AnalogIn emg0( A0 );
ThomBMT 6:bbef54156775 15 AnalogIn emg1( A1 );
ThomBMT 6:bbef54156775 16
ThomBMT 6:bbef54156775 17 Ticker sample_timer;
ThomBMT 6:bbef54156775 18 HIDScope scope( 2 );
ThomBMT 6:bbef54156775 19 DigitalOut led(LED1);
ThomBMT 6:bbef54156775 20
ThomBMT 8:81f2c8ae4427 21 Ticker Motor_Ticker1;
ThomBMT 8:81f2c8ae4427 22 Ticker Motor_Ticker2;
ThomBMT 8:81f2c8ae4427 23 Ticker EMG_Read_Ticker;
ThomBMT 8:81f2c8ae4427 24
ThomBMT 7:d57eba2aed38 25 volatile float Bicep_Right = 0.0;
ThomBMT 7:d57eba2aed38 26
ThomBMT 7:d57eba2aed38 27 void EMG_Read()
ThomBMT 7:d57eba2aed38 28 {
ThomBMT 7:d57eba2aed38 29 Bicep_Right = emg0.read();
ThomBMT 7:d57eba2aed38 30 pc.printf("%f ", Bicep_Right);
ThomBMT 7:d57eba2aed38 31 }
ThomBMT 6:bbef54156775 32
ThomBMT 6:bbef54156775 33 void sample()
ThomBMT 6:bbef54156775 34 {
ThomBMT 6:bbef54156775 35
ThomBMT 6:bbef54156775 36 scope.set(0, emg0.read() );
ThomBMT 6:bbef54156775 37 scope.set(1, emg1.read() );
ThomBMT 6:bbef54156775 38
ThomBMT 6:bbef54156775 39 scope.send();
ThomBMT 6:bbef54156775 40 led = !led;
ThomBMT 6:bbef54156775 41 }
ThomBMT 0:877f950fdfb5 42
ThomBMT 4:a1691164d879 43 int Turn_Motor1()
ThomBMT 7:d57eba2aed38 44 {
ThomBMT 7:d57eba2aed38 45
ThomBMT 7:d57eba2aed38 46
ThomBMT 5:0ae4951e9b81 47 if(!Knop1 && !Knop3 == true) // Motor 1 rotates CW
ThomBMT 0:877f950fdfb5 48 {
ThomBMT 5:0ae4951e9b81 49 PwmPin1 = fabs(0.0);
ThomBMT 0:877f950fdfb5 50 }
ThomBMT 0:877f950fdfb5 51
ThomBMT 5:0ae4951e9b81 52 else if (Knop1==false) // Motor 1 rotates CW
ThomBMT 5:0ae4951e9b81 53 {
ThomBMT 5:0ae4951e9b81 54 float u = 0.8f; //determine useful value, this is not final
ThomBMT 5:0ae4951e9b81 55 DirectionPin1 = u > 0.0f; //either true or false
ThomBMT 5:0ae4951e9b81 56 // True = CW, for False = CW
ThomBMT 5:0ae4951e9b81 57 PwmPin1 = fabs(u);
ThomBMT 5:0ae4951e9b81 58 PwmPin2 = fabs(0.0);
ThomBMT 5:0ae4951e9b81 59 }
ThomBMT 5:0ae4951e9b81 60
ThomBMT 6:bbef54156775 61 else if (Knop3==false)// We see that Motor2 keeps rotating if we leave out the "else" statement, somehow the signal leaks
ThomBMT 6:bbef54156775 62 {
ThomBMT 6:bbef54156775 63 float u = 0.8f;
ThomBMT 6:bbef54156775 64 DirectionPin1 = u < 0.0f;
ThomBMT 6:bbef54156775 65 PwmPin1 = fabs(u);
ThomBMT 6:bbef54156775 66 PwmPin2 = fabs(0.0);
ThomBMT 6:bbef54156775 67 }
ThomBMT 7:d57eba2aed38 68
ThomBMT 8:81f2c8ae4427 69 else if (Bicep_Right > 0.6f)
ThomBMT 6:bbef54156775 70 {
ThomBMT 6:bbef54156775 71 float u = 0.8f; //determine useful value, this is not final
ThomBMT 6:bbef54156775 72 DirectionPin1 = u > 0.0f; //either true or false
ThomBMT 6:bbef54156775 73 // True = CW, for False = CW
ThomBMT 6:bbef54156775 74 PwmPin1 = fabs(u);
ThomBMT 6:bbef54156775 75 PwmPin2 = fabs(0.0);
ThomBMT 6:bbef54156775 76 }
ThomBMT 7:d57eba2aed38 77
ThomBMT 4:a1691164d879 78 else
ThomBMT 4:a1691164d879 79 {
ThomBMT 4:a1691164d879 80 float u = 0.0f;
ThomBMT 4:a1691164d879 81 PwmPin1 = fabs(u);
ThomBMT 4:a1691164d879 82 }
ThomBMT 4:a1691164d879 83 return 0;
ThomBMT 4:a1691164d879 84 }
ThomBMT 4:a1691164d879 85
ThomBMT 4:a1691164d879 86 int Turn_Motor2()
ThomBMT 4:a1691164d879 87 {
ThomBMT 5:0ae4951e9b81 88 if (!Knop2 && !Knop4 == true)
ThomBMT 4:a1691164d879 89 {
ThomBMT 5:0ae4951e9b81 90 PwmPin2 = fabs(0.0);
ThomBMT 4:a1691164d879 91 }
ThomBMT 4:a1691164d879 92
ThomBMT 6:bbef54156775 93 else if (Knop2==false)
ThomBMT 6:bbef54156775 94 {
ThomBMT 6:bbef54156775 95 float u = 0.8f;
ThomBMT 6:bbef54156775 96 DirectionPin2 = u < 0.0f;
ThomBMT 6:bbef54156775 97 PwmPin1 = fabs(0.0);
ThomBMT 6:bbef54156775 98 PwmPin2 = fabs(u);
ThomBMT 6:bbef54156775 99 }
ThomBMT 6:bbef54156775 100
ThomBMT 0:877f950fdfb5 101 else if (Knop4==false)
ThomBMT 0:877f950fdfb5 102 {
ThomBMT 0:877f950fdfb5 103 float u = 0.8f;
ThomBMT 0:877f950fdfb5 104 DirectionPin2 = u > 0.0f;
ThomBMT 0:877f950fdfb5 105 PwmPin1 = fabs(0.0);
ThomBMT 0:877f950fdfb5 106 PwmPin2 = fabs(u);
ThomBMT 0:877f950fdfb5 107
ThomBMT 0:877f950fdfb5 108 }
ThomBMT 0:877f950fdfb5 109
ThomBMT 0:877f950fdfb5 110 else
ThomBMT 0:877f950fdfb5 111 {
ThomBMT 0:877f950fdfb5 112 float u = 0.0f;
ThomBMT 4:a1691164d879 113 PwmPin2 = fabs(u);
ThomBMT 0:877f950fdfb5 114 }
ThomBMT 4:a1691164d879 115 return 0;
ThomBMT 4:a1691164d879 116 }
ThomBMT 5:0ae4951e9b81 117
ThomBMT 7:d57eba2aed38 118 int main()//Ticker toevoegen
ThomBMT 4:a1691164d879 119 {
ThomBMT 4:a1691164d879 120 pc.baud(115200);
ThomBMT 5:0ae4951e9b81 121 sample_timer.attach(&sample, 0.002);
ThomBMT 4:a1691164d879 122 PwmPin1.period_us(120); //60 microseconds pwm period, 16.7 kHz
ThomBMT 7:d57eba2aed38 123
ThomBMT 8:81f2c8ae4427 124 EMG_Read_Ticker.attach(&EMG_Read, 0.002);
ThomBMT 8:81f2c8ae4427 125
ThomBMT 7:d57eba2aed38 126 while(true)
ThomBMT 6:bbef54156775 127 {
ThomBMT 4:a1691164d879 128 Turn_Motor1();
ThomBMT 7:d57eba2aed38 129 Turn_Motor2();
ThomBMT 7:d57eba2aed38 130 pc.printf("%f ", Bicep_Right);
ThomBMT 0:877f950fdfb5 131 }
ThomBMT 0:877f950fdfb5 132 }