Embedded Systems / Mbed OS Motor

Version 1

main.cpp@7:04dff5b8490c, 2017-03-16 (annotated)

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