Rami Kalai / Mbed OS MotorController

Karaoke Killers MotorController

Fork of ES_CW2_Starter by Edward Stott

new.cpp@3:7cc2b48b9a4c, 2017-03-17 (annotated)

Committer:
ramikalai
Date:
Fri Mar 17 17:11:54 2017 +0000
Revision:
3:7cc2b48b9a4c
1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ramikalai 3:7cc2b48b9a4c 1 #include "mbed.h"
ramikalai 3:7cc2b48b9a4c 2 #include "rtos.h"
ramikalai 3:7cc2b48b9a4c 3
ramikalai 3:7cc2b48b9a4c 4 //Photointerrupter input pins
ramikalai 3:7cc2b48b9a4c 5 #define I1pin D2
ramikalai 3:7cc2b48b9a4c 6 #define I2pin D11
ramikalai 3:7cc2b48b9a4c 7 #define I3pin D12
ramikalai 3:7cc2b48b9a4c 8
ramikalai 3:7cc2b48b9a4c 9 //Incremental encoder input pins
ramikalai 3:7cc2b48b9a4c 10 #define CHA D7
ramikalai 3:7cc2b48b9a4c 11 #define CHB D8
ramikalai 3:7cc2b48b9a4c 12
ramikalai 3:7cc2b48b9a4c 13 //Motor Drive output pins //Mask in output byte
ramikalai 3:7cc2b48b9a4c 14 #define L1Lpin D4 //0x01
ramikalai 3:7cc2b48b9a4c 15 #define L1Hpin D5 //0x02
ramikalai 3:7cc2b48b9a4c 16 #define L2Lpin D3 //0x04
ramikalai 3:7cc2b48b9a4c 17 #define L2Hpin D6 //0x08
ramikalai 3:7cc2b48b9a4c 18 #define L3Lpin D9 //0x10
ramikalai 3:7cc2b48b9a4c 19 #define L3Hpin D10 //0x20
ramikalai 3:7cc2b48b9a4c 20
ramikalai 3:7cc2b48b9a4c 21 //Mapping from sequential drive states to motor phase outputs
ramikalai 3:7cc2b48b9a4c 22 /*
ramikalai 3:7cc2b48b9a4c 23 State L1 L2 L3
ramikalai 3:7cc2b48b9a4c 24 0 H - L
ramikalai 3:7cc2b48b9a4c 25 1 - H L
ramikalai 3:7cc2b48b9a4c 26 2 L H -
ramikalai 3:7cc2b48b9a4c 27 3 L - H
ramikalai 3:7cc2b48b9a4c 28 4 - L H
ramikalai 3:7cc2b48b9a4c 29 5 H L -
ramikalai 3:7cc2b48b9a4c 30 6 - - -
ramikalai 3:7cc2b48b9a4c 31 7 - - -
ramikalai 3:7cc2b48b9a4c 32 */
ramikalai 3:7cc2b48b9a4c 33 //Drive state to output table
ramikalai 3:7cc2b48b9a4c 34 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
ramikalai 3:7cc2b48b9a4c 35
ramikalai 3:7cc2b48b9a4c 36 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
ramikalai 3:7cc2b48b9a4c 37 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
ramikalai 3:7cc2b48b9a4c 38 //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
ramikalai 3:7cc2b48b9a4c 39
ramikalai 3:7cc2b48b9a4c 40 //Phase lead to make motor spin
ramikalai 3:7cc2b48b9a4c 41 const int8_t lead = -2; //2 for forwards, -2 for backwards
ramikalai 3:7cc2b48b9a4c 42
ramikalai 3:7cc2b48b9a4c 43 //Status LED
ramikalai 3:7cc2b48b9a4c 44 DigitalOut led1(LED1);
ramikalai 3:7cc2b48b9a4c 45
ramikalai 3:7cc2b48b9a4c 46 //Define Photointerrupter inputs as Interrupts
ramikalai 3:7cc2b48b9a4c 47 InterruptIn I1 (I1pin);
ramikalai 3:7cc2b48b9a4c 48 InterruptIn I2 (I2pin);
ramikalai 3:7cc2b48b9a4c 49 InterruptIn I3 (I3pin);
ramikalai 3:7cc2b48b9a4c 50
ramikalai 3:7cc2b48b9a4c 51 //Globals
ramikalai 3:7cc2b48b9a4c 52 volatile int8_t intState = 0;
ramikalai 3:7cc2b48b9a4c 53 volatile int8_t intStateOld = 0;
ramikalai 3:7cc2b48b9a4c 54
ramikalai 3:7cc2b48b9a4c 55
ramikalai 3:7cc2b48b9a4c 56 //Motor Drive outputs
ramikalai 3:7cc2b48b9a4c 57 DigitalOut L1L(L1Lpin);
ramikalai 3:7cc2b48b9a4c 58 DigitalOut L1H(L1Hpin);
ramikalai 3:7cc2b48b9a4c 59 DigitalOut L2L(L2Lpin);
ramikalai 3:7cc2b48b9a4c 60 DigitalOut L2H(L2Hpin);
ramikalai 3:7cc2b48b9a4c 61 DigitalOut L3L(L3Lpin);
ramikalai 3:7cc2b48b9a4c 62 DigitalOut L3H(L3Hpin);
ramikalai 3:7cc2b48b9a4c 63
ramikalai 3:7cc2b48b9a4c 64 //Serial Comms
ramikalai 3:7cc2b48b9a4c 65 Serial pc(USBTX, USBRX);
ramikalai 3:7cc2b48b9a4c 66
ramikalai 3:7cc2b48b9a4c 67
ramikalai 3:7cc2b48b9a4c 68 void motorOut(int8_t driveState){
ramikalai 3:7cc2b48b9a4c 69
ramikalai 3:7cc2b48b9a4c 70 //Lookup the output byte from the drive state.
ramikalai 3:7cc2b48b9a4c 71 int8_t driveOut = driveTable[driveState & 0x07];
ramikalai 3:7cc2b48b9a4c 72
ramikalai 3:7cc2b48b9a4c 73 //pc.printf("test: %x\n", driveState & 0x07);
ramikalai 3:7cc2b48b9a4c 74
ramikalai 3:7cc2b48b9a4c 75 //Turn off first
ramikalai 3:7cc2b48b9a4c 76 if (~driveOut & 0x01) L1L = 0;
ramikalai 3:7cc2b48b9a4c 77 if (~driveOut & 0x02) L1H = 1;
ramikalai 3:7cc2b48b9a4c 78 if (~driveOut & 0x04) L2L = 0;
ramikalai 3:7cc2b48b9a4c 79 if (~driveOut & 0x08) L2H = 1;
ramikalai 3:7cc2b48b9a4c 80 if (~driveOut & 0x10) L3L = 0;
ramikalai 3:7cc2b48b9a4c 81 if (~driveOut & 0x20) L3H = 1;
ramikalai 3:7cc2b48b9a4c 82
ramikalai 3:7cc2b48b9a4c 83 //Then turn on
ramikalai 3:7cc2b48b9a4c 84 if (driveOut & 0x01) L1L = 1;
ramikalai 3:7cc2b48b9a4c 85 if (driveOut & 0x02) L1H = 0;
ramikalai 3:7cc2b48b9a4c 86 if (driveOut & 0x04) L2L = 1;
ramikalai 3:7cc2b48b9a4c 87 if (driveOut & 0x08) L2H = 0;
ramikalai 3:7cc2b48b9a4c 88 if (driveOut & 0x10) L3L = 1;
ramikalai 3:7cc2b48b9a4c 89 if (driveOut & 0x20) L3H = 0;
ramikalai 3:7cc2b48b9a4c 90 }
ramikalai 3:7cc2b48b9a4c 91
ramikalai 3:7cc2b48b9a4c 92 //Convert photointerrupter inputs to a rotor state
ramikalai 3:7cc2b48b9a4c 93 void readRotorState(){
ramikalai 3:7cc2b48b9a4c 94 intStateOld = intState;
ramikalai 3:7cc2b48b9a4c 95 intState = stateMap[I1.read() + 2*I2.read() + 4*I3.read()];
ramikalai 3:7cc2b48b9a4c 96
ramikalai 3:7cc2b48b9a4c 97 }
ramikalai 3:7cc2b48b9a4c 98
ramikalai 3:7cc2b48b9a4c 99 inline int8_t readRotorState1(){
ramikalai 3:7cc2b48b9a4c 100 return stateMap[I1.read() + 2*I2.read() + 4*I3.read()];
ramikalai 3:7cc2b48b9a4c 101 }
ramikalai 3:7cc2b48b9a4c 102
ramikalai 3:7cc2b48b9a4c 103
ramikalai 3:7cc2b48b9a4c 104 //Basic synchronisation routine
ramikalai 3:7cc2b48b9a4c 105 int8_t motorHome() {
ramikalai 3:7cc2b48b9a4c 106
ramikalai 3:7cc2b48b9a4c 107 //Put the motor in drive state 0 and wait for it to stabilise
ramikalai 3:7cc2b48b9a4c 108 motorOut(0);
ramikalai 3:7cc2b48b9a4c 109 wait(1.0);
ramikalai 3:7cc2b48b9a4c 110
ramikalai 3:7cc2b48b9a4c 111 //Get the rotor state
ramikalai 3:7cc2b48b9a4c 112 return readRotorState1();
ramikalai 3:7cc2b48b9a4c 113 }
ramikalai 3:7cc2b48b9a4c 114
ramikalai 3:7cc2b48b9a4c 115
ramikalai 3:7cc2b48b9a4c 116 int main() {
ramikalai 3:7cc2b48b9a4c 117
ramikalai 3:7cc2b48b9a4c 118 //Attach ISR to interruptIns
ramikalai 3:7cc2b48b9a4c 119 I1.rise(&readRotorState);
ramikalai 3:7cc2b48b9a4c 120 I2.rise(&readRotorState);
ramikalai 3:7cc2b48b9a4c 121 I3.rise(&readRotorState);
ramikalai 3:7cc2b48b9a4c 122
ramikalai 3:7cc2b48b9a4c 123
ramikalai 3:7cc2b48b9a4c 124 int8_t orState = 0; //Rotot offset at motor state 0
ramikalai 3:7cc2b48b9a4c 125 //Run the motor synchronisation
ramikalai 3:7cc2b48b9a4c 126 orState = motorHome();
ramikalai 3:7cc2b48b9a4c 127
ramikalai 3:7cc2b48b9a4c 128 readRotorState(); //To ensure that the correct initial value for intState is used
ramikalai 3:7cc2b48b9a4c 129
ramikalai 3:7cc2b48b9a4c 130
ramikalai 3:7cc2b48b9a4c 131 //orState is subtracted from future rotor state inputs to align rotor and motor states
ramikalai 3:7cc2b48b9a4c 132 //Set the motor outputs accordingly to spin the motor
ramikalai 3:7cc2b48b9a4c 133 while (1) {
ramikalai 3:7cc2b48b9a4c 134 if (intState != intStateOld) {
ramikalai 3:7cc2b48b9a4c 135 motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
ramikalai 3:7cc2b48b9a4c 136 }
ramikalai 3:7cc2b48b9a4c 137 }
ramikalai 3:7cc2b48b9a4c 138
ramikalai 3:7cc2b48b9a4c 139 }