Embedded Systems / Mbed OS Motor

Version 1

main.cpp@2:eff06aa2a885, 2017-03-16 (annotated)

Committer:
po514
Date:
Thu Mar 16 11:33:57 2017 +0000
Revision:
2:eff06aa2a885
Parent:
1:05d69ef9c7e4
Child:
3:69c670ed6382
changed period

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;
Iswanto 0:bb40c446eb83 44 volatile float freq;
Iswanto 0:bb40c446eb83 45
Iswanto 0:bb40c446eb83 46 //Status LED
Iswanto 0:bb40c446eb83 47 DigitalOut led1(LED1);
Iswanto 0:bb40c446eb83 48 DigitalOut led2 (LED2);
Iswanto 0:bb40c446eb83 49 DigitalOut led3 (LED3);
Iswanto 0:bb40c446eb83 50
Iswanto 0:bb40c446eb83 51 //Photointerrupter inputs
Iswanto 0:bb40c446eb83 52 InterruptIn I1intr(I1pin);
Iswanto 0:bb40c446eb83 53 DigitalIn I1(I1pin);
Iswanto 0:bb40c446eb83 54 DigitalIn I2(I2pin);
Iswanto 0:bb40c446eb83 55 DigitalIn I3(I3pin);
Iswanto 0:bb40c446eb83 56
Iswanto 0:bb40c446eb83 57 Ticker tick;
Iswanto 0:bb40c446eb83 58 Thread th1;
Iswanto 0:bb40c446eb83 59 Thread th2;
Iswanto 0:bb40c446eb83 60 Timer t;
Iswanto 0:bb40c446eb83 61
Iswanto 0:bb40c446eb83 62 Serial pc(SERIAL_TX,SERIAL_RX);
Iswanto 0:bb40c446eb83 63 //Motor Drive outputs
Iswanto 0:bb40c446eb83 64 PwmOut L1L(L1Lpin);
Iswanto 0:bb40c446eb83 65 PwmOut L1H(L1Hpin);
Iswanto 0:bb40c446eb83 66 PwmOut L2L(L2Lpin);
Iswanto 0:bb40c446eb83 67 PwmOut L2H(L2Hpin);
Iswanto 0:bb40c446eb83 68 PwmOut L3L(L3Lpin);
Iswanto 0:bb40c446eb83 69 PwmOut L3H(L3Hpin);
Iswanto 0:bb40c446eb83 70
Iswanto 0:bb40c446eb83 71 //Set a given drive state
Iswanto 0:bb40c446eb83 72
Iswanto 0:bb40c446eb83 73 void blinkLED2(){
Iswanto 0:bb40c446eb83 74 while(1){
Iswanto 0:bb40c446eb83 75 led1 = 0;
Iswanto 0:bb40c446eb83 76 wait(0.5);
Iswanto 0:bb40c446eb83 77 led1 = 1;
Iswanto 0:bb40c446eb83 78 wait(0.5);
Iswanto 0:bb40c446eb83 79 }
Iswanto 0:bb40c446eb83 80 }
Iswanto 0:bb40c446eb83 81
Iswanto 0:bb40c446eb83 82 void blinkLED3(){
Iswanto 0:bb40c446eb83 83 while(1){
Iswanto 0:bb40c446eb83 84 led3 = 0;
Iswanto 0:bb40c446eb83 85 wait(0.1);
Iswanto 0:bb40c446eb83 86 led3 = 1;
Iswanto 0:bb40c446eb83 87 wait(0.1);
Iswanto 0:bb40c446eb83 88 }
Iswanto 0:bb40c446eb83 89 }
Iswanto 0:bb40c446eb83 90
Iswanto 0:bb40c446eb83 91 void toggleLED(){
Iswanto 0:bb40c446eb83 92 led3 = !led3;
Iswanto 0:bb40c446eb83 93 float speedTime;
Iswanto 0:bb40c446eb83 94 speedTime = t.read();
Iswanto 0:bb40c446eb83 95 revPerMin = 1.0/(speedTime - revTime);
Iswanto 0:bb40c446eb83 96 revTime = speedTime;
Iswanto 0:bb40c446eb83 97 pc.printf("speed: %f \n\r", revPerMin);
Iswanto 0:bb40c446eb83 98 }
Iswanto 0:bb40c446eb83 99
Iswanto 0:bb40c446eb83 100 void decrease(){
Iswanto 0:bb40c446eb83 101 freq -= 0.05;
Iswanto 0:bb40c446eb83 102 pc.printf("current value: %f", freq);
Iswanto 0:bb40c446eb83 103 if (freq < 0.2){
Iswanto 0:bb40c446eb83 104 freq = 1;
Iswanto 0:bb40c446eb83 105 }
Iswanto 0:bb40c446eb83 106 }
Iswanto 0:bb40c446eb83 107
Iswanto 0:bb40c446eb83 108
Iswanto 0:bb40c446eb83 109 void motorOut(int8_t driveState, float dutyCycle){
Iswanto 0:bb40c446eb83 110 //float dutyCycle = 1.0f;
Iswanto 0:bb40c446eb83 111 //Lookup the output byte from the drive state.
Iswanto 0:bb40c446eb83 112 int8_t driveOut = driveTable[driveState & 0x07];
Iswanto 0:bb40c446eb83 113
Iswanto 0:bb40c446eb83 114 //Turn off first
Iswanto 0:bb40c446eb83 115 if (~driveOut & 0x01) L1L.write(0); // = 0
Iswanto 0:bb40c446eb83 116 if (~driveOut & 0x02) L1H.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 117 if (~driveOut & 0x04) L2L.write(0); // = 0;
Iswanto 0:bb40c446eb83 118 if (~driveOut & 0x08) L2H.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 119 if (~driveOut & 0x10) L3L.write(0);// = 0;
Iswanto 0:bb40c446eb83 120 if (~driveOut & 0x20) L3H.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 121
Iswanto 0:bb40c446eb83 122 //Then turn on
Iswanto 0:bb40c446eb83 123 if (driveOut & 0x01) L1L.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 124 if (driveOut & 0x02) L1H.write(0); // = 0;
Iswanto 0:bb40c446eb83 125 if (driveOut & 0x04) L2L.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 126 if (driveOut & 0x08) L2H.write(0);// = 0;
Iswanto 0:bb40c446eb83 127 if (driveOut & 0x10) L3L.write(dutyCycle);// = 1;
Iswanto 0:bb40c446eb83 128 if (driveOut & 0x20) L3H.write(0);// = 0;
Iswanto 0:bb40c446eb83 129 }
Iswanto 0:bb40c446eb83 130
Iswanto 0:bb40c446eb83 131 //Convert photointerrupter inputs to a rotor state
Iswanto 0:bb40c446eb83 132 inline int8_t readRotorState(){
Iswanto 0:bb40c446eb83 133 return stateMap[I1 + 2*I2 + 4*I3];
Iswanto 0:bb40c446eb83 134 }
Iswanto 0:bb40c446eb83 135
Iswanto 0:bb40c446eb83 136 //Basic synchronisation routine
Iswanto 0:bb40c446eb83 137 int8_t motorHome() {
Iswanto 0:bb40c446eb83 138 //Put the motor in drive state 0 and wait for it to stabilise
Iswanto 0:bb40c446eb83 139 motorOut(0, 1);
Iswanto 0:bb40c446eb83 140 wait(1.0);
Iswanto 0:bb40c446eb83 141
Iswanto 0:bb40c446eb83 142 //Get the rotor state
Iswanto 0:bb40c446eb83 143 return readRotorState();
Iswanto 0:bb40c446eb83 144 }
Iswanto 0:bb40c446eb83 145
Iswanto 0:bb40c446eb83 146 //Main
Iswanto 0:bb40c446eb83 147 int main() {
Iswanto 0:bb40c446eb83 148 int8_t orState = 0; //Rotot offset at motor state 0
Iswanto 0:bb40c446eb83 149
Iswanto 0:bb40c446eb83 150 //Initialise the serial port
Iswanto 0:bb40c446eb83 151 Serial pc(SERIAL_TX, SERIAL_RX);
Iswanto 0:bb40c446eb83 152 int8_t intState = 0;
Iswanto 0:bb40c446eb83 153 int8_t intStateOld = 0;
po514 2:eff06aa2a885 154 float per = 0.02f;
Iswanto 0:bb40c446eb83 155
Iswanto 0:bb40c446eb83 156 L1L.period(per);
Iswanto 0:bb40c446eb83 157 L1H.period(per);
Iswanto 0:bb40c446eb83 158 L2L.period(per);
Iswanto 0:bb40c446eb83 159 L2H.period(per);
Iswanto 0:bb40c446eb83 160 L3L.period(per);
Iswanto 0:bb40c446eb83 161 L3H.period(per);
Iswanto 0:bb40c446eb83 162 freq = 1;
Iswanto 0:bb40c446eb83 163 pc.printf("Device on \n\r");
Iswanto 0:bb40c446eb83 164
Iswanto 0:bb40c446eb83 165 //Run the motor synchronisation
Iswanto 0:bb40c446eb83 166 orState = motorHome();
Iswanto 0:bb40c446eb83 167 //orState is subtracted from future rotor state inputs to align rotor and motor states
Iswanto 0:bb40c446eb83 168 //th2.set_priority(osPriorityRealtime);
Iswanto 0:bb40c446eb83 169 //th1.start(blinkLED2);
Iswanto 0:bb40c446eb83 170 //th2.start(blinkLED3);
Iswanto 0:bb40c446eb83 171 I1intr.rise(&toggleLED);
Iswanto 0:bb40c446eb83 172 t.start();
Iswanto 0:bb40c446eb83 173 //tick.attach(&decrease, 10);
Iswanto 0:bb40c446eb83 174 //Poll the rotor state and set the motor outputs accordingly to spin the motor
Iswanto 0:bb40c446eb83 175 while (1) {
Iswanto 0:bb40c446eb83 176 intState = readRotorState();
Iswanto 0:bb40c446eb83 177 if (pc.readable()){
Iswanto 0:bb40c446eb83 178 char buffer[128];
Iswanto 0:bb40c446eb83 179
Iswanto 0:bb40c446eb83 180 pc.gets(buffer, 6);
Iswanto 0:bb40c446eb83 181 freq = atof(buffer);
Iswanto 0:bb40c446eb83 182 pc.printf("I got '%s'\n\r", buffer);
Iswanto 0:bb40c446eb83 183 pc.printf("Also in float '%f'\n\r", freq);
Iswanto 0:bb40c446eb83 184 orState = motorHome();
Iswanto 0:bb40c446eb83 185 }
Iswanto 0:bb40c446eb83 186
Iswanto 0:bb40c446eb83 187 if (intState != intStateOld) {
Iswanto 0:bb40c446eb83 188 intStateOld = intState;
Iswanto 0:bb40c446eb83 189 motorOut((intState-orState+lead+6)%6, freq); //+6 to make sure the remainder is positive
Iswanto 0:bb40c446eb83 190 }
Iswanto 0:bb40c446eb83 191 }
Iswanto 0:bb40c446eb83 192 }
Iswanto 0:bb40c446eb83 193
Iswanto 0:bb40c446eb83 194
Iswanto 1:05d69ef9c7e4 195 // hi