Brighton de Jong / Mbed 2 deprecated Turning_Motor_V1000

Facking kut filter werkt EINDELIJK !311!!111!!1!!!!!!!!!!!!!!!!!!!

Dependencies:   HIDScope MODSERIAL Matrix QEI biquadFilter mbed

Fork of Turning_Motor_V6 by Brighton de Jong

main.cpp@4:8f67b8327300, 2018-10-23 (annotated)

Committer:
ThomBMT
Date:
Tue Oct 23 13:08:22 2018 +0000
Revision:
4:8f67b8327300
Parent:
3:c8f0fc045505
Child:
5:312186a0604d
Added a switchstate machine for calibrating and homing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JurrienBos 0:4591ba678a39 1 #include "mbed.h"
JurrienBos 0:4591ba678a39 2 #include "MODSERIAL.h"
ThomBMT 2:dc9766657afb 3 #include "HIDScope.h"
ThomBMT 2:dc9766657afb 4 #include "QEI.h"
ThomBMT 2:dc9766657afb 5
JurrienBos 0:4591ba678a39 6 MODSERIAL pc(USBTX, USBRX);
JurrienBos 0:4591ba678a39 7 DigitalOut DirectionPin1(D4);
JurrienBos 1:4bf64d003f3a 8 DigitalOut DirectionPin2(D7);
JurrienBos 0:4591ba678a39 9 PwmOut PwmPin1(D5);
JurrienBos 1:4bf64d003f3a 10 PwmOut PwmPin2(D6);
JurrienBos 0:4591ba678a39 11 DigitalIn Knop1(D2);
ThomBMT 4:8f67b8327300 12 DigitalIn Knop2(D3);
JurrienBos 0:4591ba678a39 13 AnalogIn pot1 (A5);
JurrienBos 1:4bf64d003f3a 14 AnalogIn pot2 (A4);
ThomBMT 2:dc9766657afb 15 AnalogIn emg0( A0 );
ThomBMT 2:dc9766657afb 16 AnalogIn emg1( A1 );
ThomBMT 2:dc9766657afb 17 AnalogIn emg2( A2 );
ThomBMT 2:dc9766657afb 18 AnalogIn emg3( A3 );
JurrienBos 0:4591ba678a39 19
ThomBMT 4:8f67b8327300 20 Ticker StateTicker;
ThomBMT 3:c8f0fc045505 21 Ticker EncodingTicker;
ThomBMT 2:dc9766657afb 22 Ticker printTicker;
ThomBMT 2:dc9766657afb 23 Ticker EMG_Read_Ticker;
ThomBMT 2:dc9766657afb 24 Ticker sample_timer;
ThomBMT 2:dc9766657afb 25 HIDScope scope( 4 );
JurrienBos 0:4591ba678a39 26
ThomBMT 3:c8f0fc045505 27 volatile float Bicep_Right = 0.0;
ThomBMT 3:c8f0fc045505 28 volatile float Bicep_Left = 0.0;
ThomBMT 3:c8f0fc045505 29 volatile float Tricep_Right = 0.0;
ThomBMT 3:c8f0fc045505 30 volatile float Tricep_Left = 0.0;
ThomBMT 3:c8f0fc045505 31 volatile const float maxVelocity = 8.4; // in rad/s
ThomBMT 4:8f67b8327300 32 volatile const double pi = 3.14159265358979;
ThomBMT 3:c8f0fc045505 33 volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand
ThomBMT 3:c8f0fc045505 34 volatile float referenceVelocity2 = 0.5;
ThomBMT 2:dc9766657afb 35
ThomBMT 4:8f67b8327300 36 enum states{Calibration, Homing, Function};
ThomBMT 4:8f67b8327300 37
ThomBMT 4:8f67b8327300 38 volatile states Active_State = Calibration;
ThomBMT 4:8f67b8327300 39
ThomBMT 2:dc9766657afb 40 volatile int counts1;
ThomBMT 2:dc9766657afb 41 volatile int counts2;
ThomBMT 4:8f67b8327300 42
ThomBMT 3:c8f0fc045505 43 void Encoding()
ThomBMT 3:c8f0fc045505 44 {
ThomBMT 4:8f67b8327300 45
ThomBMT 4:8f67b8327300 46 QEI Encoder1(D12,D13,NC,64);
ThomBMT 3:c8f0fc045505 47 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
ThomBMT 3:c8f0fc045505 48 counts1 = Encoder1.getPulses();
ThomBMT 3:c8f0fc045505 49 counts2 = Encoder2.getPulses();
ThomBMT 3:c8f0fc045505 50
ThomBMT 4:8f67b8327300 51 float rad_m1 = ((2*pi)/32.0)* (float)counts1;
ThomBMT 4:8f67b8327300 52 float rad_m2 = ((2*pi)/32.0)* (float)counts2;
ThomBMT 4:8f67b8327300 53
ThomBMT 4:8f67b8327300 54 // pc.printf("%f & %f ....\n",rad_m1, rad_m2);
ThomBMT 3:c8f0fc045505 55 }
ThomBMT 2:dc9766657afb 56
ThomBMT 2:dc9766657afb 57 void EMG_Read()
ThomBMT 2:dc9766657afb 58 {
ThomBMT 3:c8f0fc045505 59 Bicep_Right = emg0.read();
ThomBMT 3:c8f0fc045505 60 Bicep_Left = emg1.read();
ThomBMT 3:c8f0fc045505 61 Tricep_Right = emg2.read();
ThomBMT 3:c8f0fc045505 62 Tricep_Left = emg3.read();
ThomBMT 2:dc9766657afb 63 }
ThomBMT 2:dc9766657afb 64
ThomBMT 2:dc9766657afb 65 void sample()
ThomBMT 2:dc9766657afb 66 {
ThomBMT 2:dc9766657afb 67
ThomBMT 2:dc9766657afb 68 scope.set(0, emg0.read() );
ThomBMT 2:dc9766657afb 69 scope.set(1, emg1.read() );
ThomBMT 2:dc9766657afb 70 scope.set(2, emg2.read() );
ThomBMT 2:dc9766657afb 71 scope.set(3, emg3.read() );
ThomBMT 2:dc9766657afb 72
ThomBMT 2:dc9766657afb 73 scope.send();
ThomBMT 2:dc9766657afb 74 }
ThomBMT 2:dc9766657afb 75
JurrienBos 1:4bf64d003f3a 76
JurrienBos 1:4bf64d003f3a 77 void velocity1()
JurrienBos 0:4591ba678a39 78 {
ThomBMT 3:c8f0fc045505 79 if (pot1.read()>0.5f)
JurrienBos 0:4591ba678a39 80 {
JurrienBos 0:4591ba678a39 81 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 82 referenceVelocity1 = (pot1.read()-0.5f) * 2.0f;
JurrienBos 0:4591ba678a39 83 }
JurrienBos 0:4591ba678a39 84
JurrienBos 1:4bf64d003f3a 85 else if (pot1.read() == 0.5f || !Knop1 == (pot1.read()<0.5f))
JurrienBos 0:4591ba678a39 86 {
JurrienBos 1:4bf64d003f3a 87 referenceVelocity1 = pot1.read() * 0.0f;
JurrienBos 0:4591ba678a39 88 }
JurrienBos 0:4591ba678a39 89
JurrienBos 0:4591ba678a39 90 else if (pot1.read() < 0.5f)
JurrienBos 0:4591ba678a39 91 {
JurrienBos 0:4591ba678a39 92 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 93 referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
JurrienBos 0:4591ba678a39 94 }
JurrienBos 0:4591ba678a39 95 }
JurrienBos 0:4591ba678a39 96
JurrienBos 1:4bf64d003f3a 97 void velocity2()
JurrienBos 1:4bf64d003f3a 98 {
JurrienBos 1:4bf64d003f3a 99 if (pot2.read()>0.5f)
JurrienBos 1:4bf64d003f3a 100 {
JurrienBos 1:4bf64d003f3a 101 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 102 referenceVelocity2 = (pot2.read()-0.5f) * 2.0f;
JurrienBos 1:4bf64d003f3a 103 }
JurrienBos 1:4bf64d003f3a 104
JurrienBos 1:4bf64d003f3a 105 else if (pot2.read() == 0.5f)
JurrienBos 1:4bf64d003f3a 106 {
JurrienBos 1:4bf64d003f3a 107 referenceVelocity2 = pot2.read() * 0.0f;
JurrienBos 1:4bf64d003f3a 108 }
JurrienBos 1:4bf64d003f3a 109
JurrienBos 1:4bf64d003f3a 110 else if (pot2.read() < 0.5f)
JurrienBos 1:4bf64d003f3a 111 {
JurrienBos 1:4bf64d003f3a 112 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 113 referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
JurrienBos 1:4bf64d003f3a 114 }
JurrienBos 1:4bf64d003f3a 115 }
JurrienBos 1:4bf64d003f3a 116
JurrienBos 1:4bf64d003f3a 117 void motor1()
JurrienBos 0:4591ba678a39 118 {
JurrienBos 1:4bf64d003f3a 119 float u = referenceVelocity1;
JurrienBos 0:4591ba678a39 120 DirectionPin1 = u < 0.0f;
JurrienBos 0:4591ba678a39 121 PwmPin1 = fabs(u);
JurrienBos 0:4591ba678a39 122 }
JurrienBos 0:4591ba678a39 123
JurrienBos 1:4bf64d003f3a 124 void motor2()
JurrienBos 1:4bf64d003f3a 125 {
JurrienBos 1:4bf64d003f3a 126 float u = referenceVelocity2;
JurrienBos 1:4bf64d003f3a 127 DirectionPin2 = u > 0.0f;
JurrienBos 1:4bf64d003f3a 128 PwmPin2 = fabs(u);
JurrienBos 1:4bf64d003f3a 129 }
JurrienBos 0:4591ba678a39 130
ThomBMT 2:dc9766657afb 131 void Printing()
ThomBMT 2:dc9766657afb 132 {
ThomBMT 2:dc9766657afb 133 float v1 = fabs(referenceVelocity1) * maxVelocity;
ThomBMT 2:dc9766657afb 134 float v2 = fabs(referenceVelocity2) * maxVelocity;
ThomBMT 2:dc9766657afb 135
ThomBMT 2:dc9766657afb 136 //eventueel nog counts -> rad/s
ThomBMT 2:dc9766657afb 137
ThomBMT 2:dc9766657afb 138 //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2);
ThomBMT 3:c8f0fc045505 139 pc.printf("%f ", counts1);
ThomBMT 2:dc9766657afb 140 }
ThomBMT 2:dc9766657afb 141
ThomBMT 4:8f67b8327300 142 void StateMachine()
ThomBMT 4:8f67b8327300 143 {
ThomBMT 4:8f67b8327300 144 switch (Active_State)
ThomBMT 4:8f67b8327300 145 {
ThomBMT 4:8f67b8327300 146 case Calibration:
ThomBMT 4:8f67b8327300 147 //calibration actions
ThomBMT 4:8f67b8327300 148 //pc.printf("Calibration State");
ThomBMT 4:8f67b8327300 149 if (Knop1==false)
ThomBMT 4:8f67b8327300 150 {
ThomBMT 4:8f67b8327300 151 pc.printf("Entering Homing state \n");
ThomBMT 4:8f67b8327300 152 Active_State = Homing;
ThomBMT 4:8f67b8327300 153 }
ThomBMT 4:8f67b8327300 154 break;
ThomBMT 4:8f67b8327300 155
ThomBMT 4:8f67b8327300 156 case Homing:
ThomBMT 4:8f67b8327300 157 // Homing actions
ThomBMT 4:8f67b8327300 158 //pc.printf("Homing State");
ThomBMT 4:8f67b8327300 159 if (Knop2==false)
ThomBMT 4:8f67b8327300 160 {
ThomBMT 4:8f67b8327300 161 pc.printf("Entering Funtioning State \n");
ThomBMT 4:8f67b8327300 162 Active_State = Function;
ThomBMT 4:8f67b8327300 163 }
ThomBMT 4:8f67b8327300 164 break;
ThomBMT 4:8f67b8327300 165
ThomBMT 4:8f67b8327300 166 case Function:
ThomBMT 4:8f67b8327300 167 //pc.printf("Funtioning State");
ThomBMT 4:8f67b8327300 168
ThomBMT 4:8f67b8327300 169 velocity1();
ThomBMT 4:8f67b8327300 170 velocity2();
ThomBMT 4:8f67b8327300 171 motor1();
ThomBMT 4:8f67b8327300 172 motor2();
ThomBMT 4:8f67b8327300 173 break;
ThomBMT 4:8f67b8327300 174
ThomBMT 4:8f67b8327300 175 default:
ThomBMT 4:8f67b8327300 176 pc.printf("UNKNOWN COMMAND");
ThomBMT 4:8f67b8327300 177 }
ThomBMT 4:8f67b8327300 178 }
ThomBMT 4:8f67b8327300 179
JurrienBos 0:4591ba678a39 180 int main()
JurrienBos 0:4591ba678a39 181 {
JurrienBos 0:4591ba678a39 182 pc.baud(115200);
ThomBMT 4:8f67b8327300 183 PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz
ThomBMT 2:dc9766657afb 184
ThomBMT 3:c8f0fc045505 185 EncodingTicker.attach(&Encoding, 0.002);
ThomBMT 2:dc9766657afb 186
ThomBMT 2:dc9766657afb 187 sample_timer.attach(&sample, 0.002);
ThomBMT 2:dc9766657afb 188 EMG_Read_Ticker.attach(&EMG_Read, 0.002);
ThomBMT 2:dc9766657afb 189
ThomBMT 4:8f67b8327300 190 StateTicker.attach(StateMachine, 0.002);
ThomBMT 4:8f67b8327300 191
ThomBMT 4:8f67b8327300 192 //printTicker.attach(&Printing, 2.0);
ThomBMT 2:dc9766657afb 193
JurrienBos 0:4591ba678a39 194 while(true)
JurrienBos 0:4591ba678a39 195 {
JurrienBos 0:4591ba678a39 196 }
JurrienBos 1:4bf64d003f3a 197 }