Embedded Systems / Mbed OS Motor

Version 1

main.cpp@4:bdd445879de2, 2017-03-16 (annotated)

Committer:
po514
Date:
Thu Mar 16 12:20:01 2017 +0000
Revision:
4:bdd445879de2
Parent:
3:69c670ed6382
Child:
5:8269e635183f
updated thread for control loop

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