Thom Kuenen / Mbed 2 deprecated Turning_Motor_V3

Project of Biorobotics

Dependencies:   HIDScope MODSERIAL QEI mbed biquadFilter

Fork of TutorialPES by Jurriën Bos

main.cpp@7:439940ae1197, 2018-10-26 (annotated)

Committer:
ThomBMT
Date:
Fri Oct 26 07:58:07 2018 +0000
Revision:
7:439940ae1197
Parent:
6:056ad27636ff
Child:
8:e8734a254818
Encoder now works and outputs radians

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