Embedded Systems / Mbed OS Motor

Version 1

main.cpp@6:d881a556eb9a, 2017-03-16 (annotated)

Committer:
po514
Date:
Thu Mar 16 13:03:08 2017 +0000
Revision:
6:d881a556eb9a
Parent:
5:8269e635183f
Child:
7:04dff5b8490c
wait time in controlLoopThread updated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Iswanto 0:bb40c446eb83 1 #include "mbed.h"
Iswanto 0:bb40c446eb83 2
Iswanto 0:bb40c446eb83 3 //Photointerrupter input pins
Iswanto 0:bb40c446eb83 4 #define I1pin D2
Iswanto 0:bb40c446eb83 5 #define I2pin D11
Iswanto 0:bb40c446eb83 6 #define I3pin D12
Iswanto 0:bb40c446eb83 7
Iswanto 0:bb40c446eb83 8 //Incremental encoder input pins
Iswanto 0:bb40c446eb83 9 #define CHA D7
Iswanto 0:bb40c446eb83 10 #define CHB D8
Iswanto 0:bb40c446eb83 11
Iswanto 0:bb40c446eb83 12 //Motor Drive output pins //Mask in output byte
Iswanto 0:bb40c446eb83 13 #define L1Lpin D4 //0x01
Iswanto 0:bb40c446eb83 14 #define L1Hpin D5 //0x02
Iswanto 0:bb40c446eb83 15 #define L2Lpin D3 //0x04
Iswanto 0:bb40c446eb83 16 #define L2Hpin D6 //0x08
Iswanto 0:bb40c446eb83 17 #define L3Lpin D9 //0x10
Iswanto 0:bb40c446eb83 18 #define L3Hpin D10 //0x20
Iswanto 0:bb40c446eb83 19
Iswanto 0:bb40c446eb83 20 //Mapping from sequential drive states to motor phase outputs
Iswanto 0:bb40c446eb83 21 /*
Iswanto 0:bb40c446eb83 22 State L1 L2 L3
Iswanto 0:bb40c446eb83 23 0 H - L
Iswanto 0:bb40c446eb83 24 1 - H L
Iswanto 0:bb40c446eb83 25 2 L H -
Iswanto 0:bb40c446eb83 26 3 L - H
Iswanto 0:bb40c446eb83 27 4 - L H
Iswanto 0:bb40c446eb83 28 5 H L -
Iswanto 0:bb40c446eb83 29 6 - - -
Iswanto 0:bb40c446eb83 30 7 - - -
Iswanto 0:bb40c446eb83 31 */
Iswanto 0:bb40c446eb83 32 //Drive state to output table
Iswanto 0:bb40c446eb83 33 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
Iswanto 0:bb40c446eb83 34
Iswanto 0:bb40c446eb83 35 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
Iswanto 0:bb40c446eb83 36 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
Iswanto 0:bb40c446eb83 37 //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
Iswanto 0:bb40c446eb83 38
Iswanto 0:bb40c446eb83 39 //Phase lead to make motor spin
Iswanto 0:bb40c446eb83 40 const int8_t lead = 2; //2 for forwards, -2 for backwards
Iswanto 0:bb40c446eb83 41
Iswanto 0:bb40c446eb83 42 volatile float revTime;
Iswanto 0:bb40c446eb83 43 volatile float revPerMin;
po514 3:69c670ed6382 44 volatile float dutyCycle;
po514 4:bdd445879de2 45 volatile float targetSpeed;
Iswanto 0:bb40c446eb83 46
po514 5:8269e635183f 47 volatile float timeMap[] = {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 };
po514 5:8269e635183f 48
Iswanto 0:bb40c446eb83 49 //Status LED
Iswanto 0:bb40c446eb83 50 DigitalOut led1(LED1);
Iswanto 0:bb40c446eb83 51 DigitalOut led2 (LED2);
Iswanto 0:bb40c446eb83 52 DigitalOut led3 (LED3);
Iswanto 0:bb40c446eb83 53
Iswanto 0:bb40c446eb83 54 //Photointerrupter inputs
Iswanto 0:bb40c446eb83 55 InterruptIn I1intr(I1pin);
po514 5:8269e635183f 56 InterruptIn I2intr(I2pin);
po514 5:8269e635183f 57 InterruptIn I3intr(I3pin);
po514 5:8269e635183f 58
Iswanto 0:bb40c446eb83 59 DigitalIn I1(I1pin);
Iswanto 0:bb40c446eb83 60 DigitalIn I2(I2pin);
Iswanto 0:bb40c446eb83 61 DigitalIn I3(I3pin);
Iswanto 0:bb40c446eb83 62
Iswanto 0:bb40c446eb83 63 Ticker tick;
Iswanto 0:bb40c446eb83 64 Thread th1;
po514 4:bdd445879de2 65 Thread controlLoopThread;
Iswanto 0:bb40c446eb83 66 Timer t;
Iswanto 0:bb40c446eb83 67
Iswanto 0:bb40c446eb83 68 Serial pc(SERIAL_TX,SERIAL_RX);
Iswanto 0:bb40c446eb83 69 //Motor Drive outputs
Iswanto 0:bb40c446eb83 70 PwmOut L1L(L1Lpin);
Iswanto 0:bb40c446eb83 71 PwmOut L1H(L1Hpin);
Iswanto 0:bb40c446eb83 72 PwmOut L2L(L2Lpin);
Iswanto 0:bb40c446eb83 73 PwmOut L2H(L2Hpin);
Iswanto 0:bb40c446eb83 74 PwmOut L3L(L3Lpin);
Iswanto 0:bb40c446eb83 75 PwmOut L3H(L3Hpin);
Iswanto 0:bb40c446eb83 76
Iswanto 0:bb40c446eb83 77 //Set a given drive state
Iswanto 0:bb40c446eb83 78
Iswanto 0:bb40c446eb83 79 void blinkLED2(){
Iswanto 0:bb40c446eb83 80 while(1){
Iswanto 0:bb40c446eb83 81 led1 = 0;
Iswanto 0:bb40c446eb83 82 wait(0.5);
Iswanto 0:bb40c446eb83 83 led1 = 1;
Iswanto 0:bb40c446eb83 84 wait(0.5);
Iswanto 0:bb40c446eb83 85 }
Iswanto 0:bb40c446eb83 86 }
Iswanto 0:bb40c446eb83 87
Iswanto 0:bb40c446eb83 88 void blinkLED3(){
Iswanto 0:bb40c446eb83 89 while(1){
Iswanto 0:bb40c446eb83 90 led3 = 0;
Iswanto 0:bb40c446eb83 91 wait(0.1);
Iswanto 0:bb40c446eb83 92 led3 = 1;
Iswanto 0:bb40c446eb83 93 wait(0.1);
Iswanto 0:bb40c446eb83 94 }
Iswanto 0:bb40c446eb83 95 }
Iswanto 0:bb40c446eb83 96
po514 4:bdd445879de2 97 void mesRot(){
Iswanto 0:bb40c446eb83 98 led3 = !led3;
Iswanto 0:bb40c446eb83 99 float speedTime;
Iswanto 0:bb40c446eb83 100 speedTime = t.read();
po514 5:8269e635183f 101 revPerMin = 1.0/(speedTime - timeMap[I1 + 2*I2 + 4*I3]);
po514 5:8269e635183f 102 timeMap[I1 + 2*I2 + 4*I3] = speedTime;
Iswanto 0:bb40c446eb83 103 }
Iswanto 0:bb40c446eb83 104
po514 4:bdd445879de2 105 void controlLoop(void){
po514 4:bdd445879de2 106 while(true){
po514 4:bdd445879de2 107 float error = targetSpeed - revPerMin;
po514 4:bdd445879de2 108 float k = 10.0;
po514 4:bdd445879de2 109 dutyCycle = k*error;
po514 6:d881a556eb9a 110 wait(0.05);
po514 4:bdd445879de2 111 }
po514 4:bdd445879de2 112 }
po514 4:bdd445879de2 113
Iswanto 0:bb40c446eb83 114 void decrease(){
po514 3:69c670ed6382 115 dutyCycle -= 0.05;
po514 3:69c670ed6382 116 pc.printf("current value: %f", dutyCycle);
po514 3:69c670ed6382 117 if (dutyCycle < 0.2){
po514 3:69c670ed6382 118 dutyCycle = 1;
Iswanto 0:bb40c446eb83 119 }
Iswanto 0:bb40c446eb83 120 }
Iswanto 0:bb40c446eb83 121
Iswanto 0:bb40c446eb83 122
Iswanto 0:bb40c446eb83 123 void motorOut(int8_t driveState, float dutyCycle){
Iswanto 0:bb40c446eb83 124 //float dutyCycle = 1.0f;
Iswanto 0:bb40c446eb83 125 //Lookup the output byte from the drive state.
Iswanto 0:bb40c446eb83 126 int8_t driveOut = driveTable[driveState & 0x07];
Iswanto 0:bb40c446eb83 127
Iswanto 0:bb40c446eb83 128 //Turn off first
Iswanto 0:bb40c446eb83 129 if (~driveOut & 0x01) L1L.write(0); // = 0
Iswanto 0:bb40c446eb83 130 if (~driveOut & 0x02) L1H.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 131 if (~driveOut & 0x04) L2L.write(0); // = 0;
Iswanto 0:bb40c446eb83 132 if (~driveOut & 0x08) L2H.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 133 if (~driveOut & 0x10) L3L.write(0);// = 0;
Iswanto 0:bb40c446eb83 134 if (~driveOut & 0x20) L3H.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 135
Iswanto 0:bb40c446eb83 136 //Then turn on
Iswanto 0:bb40c446eb83 137 if (driveOut & 0x01) L1L.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 138 if (driveOut & 0x02) L1H.write(0); // = 0;
Iswanto 0:bb40c446eb83 139 if (driveOut & 0x04) L2L.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 140 if (driveOut & 0x08) L2H.write(0);// = 0;
Iswanto 0:bb40c446eb83 141 if (driveOut & 0x10) L3L.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 142 if (driveOut & 0x20) L3H.write(0);// = 0;
Iswanto 0:bb40c446eb83 143 }
Iswanto 0:bb40c446eb83 144
Iswanto 0:bb40c446eb83 145 //Convert photointerrupter inputs to a rotor state
Iswanto 0:bb40c446eb83 146 inline int8_t readRotorState(){
Iswanto 0:bb40c446eb83 147 return stateMap[I1 + 2*I2 + 4*I3];
Iswanto 0:bb40c446eb83 148 }
Iswanto 0:bb40c446eb83 149
Iswanto 0:bb40c446eb83 150 //Basic synchronisation routine
Iswanto 0:bb40c446eb83 151 int8_t motorHome() {
Iswanto 0:bb40c446eb83 152 //Put the motor in drive state 0 and wait for it to stabilise
Iswanto 0:bb40c446eb83 153 motorOut(0, 1);
Iswanto 0:bb40c446eb83 154 wait(1.0);
Iswanto 0:bb40c446eb83 155
Iswanto 0:bb40c446eb83 156 //Get the rotor state
Iswanto 0:bb40c446eb83 157 return readRotorState();
Iswanto 0:bb40c446eb83 158 }
Iswanto 0:bb40c446eb83 159
Iswanto 0:bb40c446eb83 160 //Main
Iswanto 0:bb40c446eb83 161 int main() {
Iswanto 0:bb40c446eb83 162 int8_t orState = 0; //Rotot offset at motor state 0
Iswanto 0:bb40c446eb83 163
Iswanto 0:bb40c446eb83 164 //Initialise the serial port
Iswanto 0:bb40c446eb83 165 Serial pc(SERIAL_TX, SERIAL_RX);
Iswanto 0:bb40c446eb83 166 int8_t intState = 0;
Iswanto 0:bb40c446eb83 167 int8_t intStateOld = 0;
po514 2:eff06aa2a885 168 float per = 0.02f;
po514 5:8269e635183f 169 targetSpeed = 3.0;
Iswanto 0:bb40c446eb83 170
Iswanto 0:bb40c446eb83 171 L1L.period(per);
Iswanto 0:bb40c446eb83 172 L1H.period(per);
Iswanto 0:bb40c446eb83 173 L2L.period(per);
Iswanto 0:bb40c446eb83 174 L2H.period(per);
Iswanto 0:bb40c446eb83 175 L3L.period(per);
Iswanto 0:bb40c446eb83 176 L3H.period(per);
po514 3:69c670ed6382 177 dutyCycle = 1;
Iswanto 0:bb40c446eb83 178 pc.printf("Device on \n\r");
Iswanto 0:bb40c446eb83 179
Iswanto 0:bb40c446eb83 180 //Run the motor synchronisation
Iswanto 0:bb40c446eb83 181 orState = motorHome();
Iswanto 0:bb40c446eb83 182 //orState is subtracted from future rotor state inputs to align rotor and motor states
Iswanto 0:bb40c446eb83 183 //th2.set_priority(osPriorityRealtime);
Iswanto 0:bb40c446eb83 184 //th1.start(blinkLED2);
Iswanto 0:bb40c446eb83 185 //th2.start(blinkLED3);
po514 5:8269e635183f 186
po514 5:8269e635183f 187 //define interrupts
po514 4:bdd445879de2 188 I1intr.rise(&mesRot); //start photoInterrupt
po514 5:8269e635183f 189 I2intr.rise(&mesRot);
po514 5:8269e635183f 190 I3intr.rise(&mesRot);
po514 5:8269e635183f 191 I1intr.fall(&mesRot);
po514 5:8269e635183f 192 I2intr.fall(&mesRot);
po514 5:8269e635183f 193 I3intr.fall(&mesRot);
po514 5:8269e635183f 194
po514 4:bdd445879de2 195 controlLoopThread.start(controlLoop); //start conrol unit thread
Iswanto 0:bb40c446eb83 196 t.start();
Iswanto 0:bb40c446eb83 197 //tick.attach(&decrease, 10);
Iswanto 0:bb40c446eb83 198 //Poll the rotor state and set the motor outputs accordingly to spin the motor
Iswanto 0:bb40c446eb83 199 while (1) {
Iswanto 0:bb40c446eb83 200 intState = readRotorState();
Iswanto 0:bb40c446eb83 201 if (pc.readable()){
Iswanto 0:bb40c446eb83 202 char buffer[128];
Iswanto 0:bb40c446eb83 203
Iswanto 0:bb40c446eb83 204 pc.gets(buffer, 6);
po514 5:8269e635183f 205 targetSpeed = atof(buffer);
po514 5:8269e635183f 206 pc.printf("I got '%s'\n\r", buffer);
po514 5:8269e635183f 207 pc.printf("Also in float '%f'\n\r", targetSpeed);
po514 5:8269e635183f 208 orState = motorHome();
Iswanto 0:bb40c446eb83 209 }
Iswanto 0:bb40c446eb83 210
Iswanto 0:bb40c446eb83 211 if (intState != intStateOld) {
Iswanto 0:bb40c446eb83 212 intStateOld = intState;
po514 3:69c670ed6382 213 motorOut((intState-orState+lead+6)%6, dutyCycle); //+6 to make sure the remainder is positive
Iswanto 0:bb40c446eb83 214 }
Iswanto 0:bb40c446eb83 215 }
Iswanto 0:bb40c446eb83 216 }
Iswanto 0:bb40c446eb83 217
Iswanto 0:bb40c446eb83 218
Iswanto 1:05d69ef9c7e4 219 // hi