Brighton de Jong / Mbed 2 deprecated Turning_Motor_V6

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

Dependencies:   HIDScope MODSERIAL Matrix QEI biquadFilter mbed

Fork of Filter by Jurriën Bos

main.cpp@5:312186a0604d, 2018-10-24 (annotated)

Committer:
ThomBMT
Date:
Wed Oct 24 10:30:47 2018 +0000
Revision:
5:312186a0604d
Parent:
4:8f67b8327300
Child:
6:056ad27636ff
Counts zijn nu werkend. Omrekenen naar rad is nog niet kloppend

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