SpinnyBoi / Mbed 2 deprecated Spinnybois

First Commit

Dependencies:   mbed Crypto_light mbed-rtos

Spin it 2 win it

main.cpp@1:a530f6235850, 2018-03-11 (annotated)

Committer:
TrebleStick
Date:
Sun Mar 11 17:31:26 2018 +0000
Revision:
1:a530f6235850
Parent:
0:88c3d6c8a4eb
Child:
2:862ee3609eee
Delay change

Who changed what in which revision?

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