Embedded Systems / Mbed OS ES_CW2_Advanced

New smaller slits code

Dependencies:   PID

Fork of ES_CW2_Starter by Edward Stott

main.cpp@0:de4320f74764, 2017-02-17 (annotated)

Committer:
estott
Date:
Fri Feb 17 22:47:01 2017 +0000
Revision:
0:de4320f74764
Child:
1:184cb0870c04
Initial Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
estott 0:de4320f74764 1 #include "mbed.h"
estott 0:de4320f74764 2 #include "rtos.h"
estott 0:de4320f74764 3
estott 0:de4320f74764 4 //Photointerrupter input pins
estott 0:de4320f74764 5 #define I1pin D2
estott 0:de4320f74764 6 #define I2pin D3
estott 0:de4320f74764 7 #define I3pin D4
estott 0:de4320f74764 8
estott 0:de4320f74764 9 //Motor Drive output pins //Mask in output byte
estott 0:de4320f74764 10 #define L1Lpin PA_6 //0x01
estott 0:de4320f74764 11 #define L1Hpin D6 //0x02
estott 0:de4320f74764 12 #define L2Lpin PA_7 //0x04
estott 0:de4320f74764 13 #define L2Hpin D8 //0x08
estott 0:de4320f74764 14 #define L3Lpin PA_4 //0x10
estott 0:de4320f74764 15 #define L3Hpin D10 //0x20
estott 0:de4320f74764 16
estott 0:de4320f74764 17 //Mapping from sequential drive states to motor phase outputs
estott 0:de4320f74764 18 /*
estott 0:de4320f74764 19 State L1 L2 L3
estott 0:de4320f74764 20 0 H - L
estott 0:de4320f74764 21 1 - H L
estott 0:de4320f74764 22 2 L H -
estott 0:de4320f74764 23 3 L - H
estott 0:de4320f74764 24 4 - L H
estott 0:de4320f74764 25 5 H L -
estott 0:de4320f74764 26 6 - - -
estott 0:de4320f74764 27 7 - - -
estott 0:de4320f74764 28 */
estott 0:de4320f74764 29 //Drive state to output table
estott 0:de4320f74764 30 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
estott 0:de4320f74764 31 //Mask to invert the outputs for high side transistors
estott 0:de4320f74764 32 const int8_t motorHmask = 0x2a;
estott 0:de4320f74764 33 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
estott 0:de4320f74764 34 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
estott 0:de4320f74764 35
estott 0:de4320f74764 36 //Status LED
estott 0:de4320f74764 37 DigitalOut led1(LED1);
estott 0:de4320f74764 38
estott 0:de4320f74764 39 //Photointerrupter inputs
estott 0:de4320f74764 40 DigitalIn I1int(I1pin);
estott 0:de4320f74764 41 DigitalIn I2int(I2pin);
estott 0:de4320f74764 42 DigitalIn I3int(I3pin);
estott 0:de4320f74764 43
estott 0:de4320f74764 44 //Motor Drive outputs
estott 0:de4320f74764 45 DigitalOut L1L(L1Lpin);
estott 0:de4320f74764 46 DigitalOut L1H(L1Hpin);
estott 0:de4320f74764 47 DigitalOut L2L(L2Lpin);
estott 0:de4320f74764 48 DigitalOut L2H(L2Hpin);
estott 0:de4320f74764 49 DigitalOut L3L(L3Lpin);
estott 0:de4320f74764 50 DigitalOut L3H(L3Hpin);
estott 0:de4320f74764 51
estott 0:de4320f74764 52 //Global state
estott 0:de4320f74764 53 int32_t outState = 0; //Current drive state
estott 0:de4320f74764 54 int32_t inState = 0; //Current rotor state
estott 0:de4320f74764 55 int32_t leadState = 0; //Offset to calculate drive state from rotor state
estott 0:de4320f74764 56
estott 0:de4320f74764 57 //Set a given drive state
estott 0:de4320f74764 58 void motorOut(int8_t driveState){
estott 0:de4320f74764 59 //First turn all phases off to prevent shoot-through
estott 0:de4320f74764 60 L1H = 1;
estott 0:de4320f74764 61 L2H = 1;
estott 0:de4320f74764 62 L3H = 1;
estott 0:de4320f74764 63
estott 0:de4320f74764 64 //Lookup the output byte from the drive state. Apply high side inversion mask
estott 0:de4320f74764 65 int8_t driveOut = driveTable[driveState & 0x07] ^ motorHmask;
estott 0:de4320f74764 66
estott 0:de4320f74764 67 //Apply the output byte to the pins
estott 0:de4320f74764 68 L1L = driveOut & 0x01;
estott 0:de4320f74764 69 L2L = driveOut & 0x04;
estott 0:de4320f74764 70 L3L = driveOut & 0x10;
estott 0:de4320f74764 71 L1H = driveOut & 0x02;
estott 0:de4320f74764 72 L2H = driveOut & 0x08;
estott 0:de4320f74764 73 L3H = driveOut & 0x20;
estott 0:de4320f74764 74 }
estott 0:de4320f74764 75
estott 0:de4320f74764 76 inline int8_t readRotorState(){
estott 0:de4320f74764 77 return stateMap[I1int + 2*I2int + 4*I3int];
estott 0:de4320f74764 78 }
estott 0:de4320f74764 79
estott 0:de4320f74764 80 //Basic synchronisation routine
estott 0:de4320f74764 81 void motorHome() {
estott 0:de4320f74764 82 //Put the motor in drive state 0 and wait for it to stabilise
estott 0:de4320f74764 83 motorOut(0);
estott 0:de4320f74764 84 wait(1.0);
estott 0:de4320f74764 85
estott 0:de4320f74764 86 //Get the rotor state
estott 0:de4320f74764 87 inState = readRotorState();
estott 0:de4320f74764 88
estott 0:de4320f74764 89 //Calculate the rotor-to-drive offset by adding two (modulo 6) the current rotor state
estott 0:de4320f74764 90 leadState = (inState+2)%6;
estott 0:de4320f74764 91 }
estott 0:de4320f74764 92
estott 0:de4320f74764 93 //Main
estott 0:de4320f74764 94 int main() {
estott 0:de4320f74764 95 //Initialise the serial port
estott 0:de4320f74764 96 serialThread.start(&serialOut);
estott 0:de4320f74764 97 Serial pc(SERIAL_TX, SERIAL_RX);
estott 0:de4320f74764 98 pc.printf("Hello\n\r");
estott 0:de4320f74764 99
estott 0:de4320f74764 100 //Run the motor synchronisation
estott 0:de4320f74764 101 motorHome();
estott 0:de4320f74764 102
estott 0:de4320f74764 103 //Poll the rotor state and set the motor outputs accordingly to spin the motor
estott 0:de4320f74764 104 while {1} {
estott 0:de4320f74764 105 motorOut(readRotorState()+leadState);
estott 0:de4320f74764 106 }
estott 0:de4320f74764 107 }
estott 0:de4320f74764 108