ES2017 coursework 2

Dependencies:   PID

Fork of ES_CW2_Starter by Edward Stott

Committer:
david_s95
Date:
Thu Mar 02 16:22:59 2017 +0000
Revision:
5:e5313b695302
Parent:
4:f8a9ce214db9
Child:
6:4edbe75736d9
Utter chaos, trying to get serial interrupt working but it's gone horribly. Don't bother unless you're feeling adventurous.

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"
david_s95 5:e5313b695302 3 #include <string>
estott 0:de4320f74764 4
estott 0:de4320f74764 5 //Photointerrupter input pins
estott 0:de4320f74764 6 #define I1pin D2
estott 2:4e88faab6988 7 #define I2pin D11
estott 2:4e88faab6988 8 #define I3pin D12
estott 2:4e88faab6988 9
estott 2:4e88faab6988 10 //Incremental encoder input pins
estott 2:4e88faab6988 11 #define CHA D7
david_s95 5:e5313b695302 12 #define CHB D8
estott 0:de4320f74764 13
estott 0:de4320f74764 14 //Motor Drive output pins //Mask in output byte
estott 2:4e88faab6988 15 #define L1Lpin D4 //0x01
estott 2:4e88faab6988 16 #define L1Hpin D5 //0x02
estott 2:4e88faab6988 17 #define L2Lpin D3 //0x04
estott 2:4e88faab6988 18 #define L2Hpin D6 //0x08
estott 2:4e88faab6988 19 #define L3Lpin D9 //0x10
estott 0:de4320f74764 20 #define L3Hpin D10 //0x20
estott 0:de4320f74764 21
david_s95 5:e5313b695302 22 //Define sized for command arrays
david_s95 5:e5313b695302 23 #define ARRAYSIZE 8
david_s95 5:e5313b695302 24
estott 0:de4320f74764 25 //Mapping from sequential drive states to motor phase outputs
estott 0:de4320f74764 26 /*
estott 0:de4320f74764 27 State L1 L2 L3
estott 0:de4320f74764 28 0 H - L
estott 0:de4320f74764 29 1 - H L
estott 0:de4320f74764 30 2 L H -
estott 0:de4320f74764 31 3 L - H
estott 0:de4320f74764 32 4 - L H
estott 0:de4320f74764 33 5 H L -
estott 0:de4320f74764 34 6 - - -
estott 0:de4320f74764 35 7 - - -
estott 0:de4320f74764 36 */
estott 0:de4320f74764 37 //Drive state to output table
estott 0:de4320f74764 38 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
estott 2:4e88faab6988 39
estott 0:de4320f74764 40 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
david_s95 5:e5313b695302 41 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
estott 2:4e88faab6988 42 //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
estott 2:4e88faab6988 43
estott 2:4e88faab6988 44 //Phase lead to make motor spin
david_s95 3:e7133505f542 45 const int8_t lead = 2; //2 for forwards, -2 for backwards
estott 0:de4320f74764 46
estott 0:de4320f74764 47 //Status LED
estott 0:de4320f74764 48 DigitalOut led1(LED1);
estott 0:de4320f74764 49
estott 0:de4320f74764 50 //Photointerrupter inputs
estott 2:4e88faab6988 51 DigitalIn I1(I1pin);
estott 2:4e88faab6988 52 DigitalIn I2(I2pin);
estott 2:4e88faab6988 53 DigitalIn I3(I3pin);
estott 0:de4320f74764 54
estott 0:de4320f74764 55 //Motor Drive outputs
estott 0:de4320f74764 56 DigitalOut L1L(L1Lpin);
estott 0:de4320f74764 57 DigitalOut L1H(L1Hpin);
estott 0:de4320f74764 58 DigitalOut L2L(L2Lpin);
estott 0:de4320f74764 59 DigitalOut L2H(L2Hpin);
estott 0:de4320f74764 60 DigitalOut L3L(L3Lpin);
estott 0:de4320f74764 61 DigitalOut L3H(L3Hpin);
david_s95 5:e5313b695302 62 DigitalOut clk(LED1);
david_s95 5:e5313b695302 63
david_s95 5:e5313b695302 64 //Timeout function for rotating at set speed
david_s95 5:e5313b695302 65 Timeout spinTimer;
david_s95 5:e5313b695302 66 float spinWait = 10;
david_s95 5:e5313b695302 67 float revsec = 0;
david_s95 5:e5313b695302 68
david_s95 5:e5313b695302 69 Serial pc(SERIAL_TX, SERIAL_RX);
david_s95 5:e5313b695302 70
david_s95 5:e5313b695302 71 int8_t orState = 0; //Rotor offset at motor state 0
david_s95 5:e5313b695302 72 int8_t intState = 0;
david_s95 5:e5313b695302 73 int8_t intStateOld = 0;
david_s95 5:e5313b695302 74
david_s95 5:e5313b695302 75 int i=0;
david_s95 5:e5313b695302 76
david_s95 5:e5313b695302 77
estott 0:de4320f74764 78
estott 0:de4320f74764 79 //Set a given drive state
david_s95 5:e5313b695302 80 void motorOut(int8_t driveState)
david_s95 5:e5313b695302 81 {
david_s95 5:e5313b695302 82
estott 2:4e88faab6988 83 //Lookup the output byte from the drive state.
estott 2:4e88faab6988 84 int8_t driveOut = driveTable[driveState & 0x07];
david_s95 5:e5313b695302 85
estott 2:4e88faab6988 86 //Turn off first
estott 2:4e88faab6988 87 if (~driveOut & 0x01) L1L = 0;
estott 2:4e88faab6988 88 if (~driveOut & 0x02) L1H = 1;
estott 2:4e88faab6988 89 if (~driveOut & 0x04) L2L = 0;
estott 2:4e88faab6988 90 if (~driveOut & 0x08) L2H = 1;
estott 2:4e88faab6988 91 if (~driveOut & 0x10) L3L = 0;
estott 2:4e88faab6988 92 if (~driveOut & 0x20) L3H = 1;
david_s95 5:e5313b695302 93
estott 2:4e88faab6988 94 //Then turn on
estott 2:4e88faab6988 95 if (driveOut & 0x01) L1L = 1;
estott 2:4e88faab6988 96 if (driveOut & 0x02) L1H = 0;
estott 2:4e88faab6988 97 if (driveOut & 0x04) L2L = 1;
estott 2:4e88faab6988 98 if (driveOut & 0x08) L2H = 0;
estott 2:4e88faab6988 99 if (driveOut & 0x10) L3L = 1;
estott 2:4e88faab6988 100 if (driveOut & 0x20) L3H = 0;
david_s95 5:e5313b695302 101 }
david_s95 5:e5313b695302 102
david_s95 5:e5313b695302 103 //Convert photointerrupter inputs to a rotor state
david_s95 5:e5313b695302 104 inline int8_t readRotorState()
david_s95 5:e5313b695302 105 {
estott 2:4e88faab6988 106 return stateMap[I1 + 2*I2 + 4*I3];
david_s95 5:e5313b695302 107 }
estott 0:de4320f74764 108
david_s95 5:e5313b695302 109 //Basic synchronisation routine
david_s95 5:e5313b695302 110 int8_t motorHome()
david_s95 5:e5313b695302 111 {
estott 0:de4320f74764 112 //Put the motor in drive state 0 and wait for it to stabilise
estott 0:de4320f74764 113 motorOut(0);
estott 0:de4320f74764 114 wait(1.0);
david_s95 5:e5313b695302 115
estott 0:de4320f74764 116 //Get the rotor state
estott 2:4e88faab6988 117 return readRotorState();
estott 0:de4320f74764 118 }
david_s95 5:e5313b695302 119
david_s95 5:e5313b695302 120 void fixedSpeed()
david_s95 5:e5313b695302 121 {
david_s95 5:e5313b695302 122 intState = readRotorState();
david_s95 5:e5313b695302 123 motorOut((intState-orState+lead+6)%6);
david_s95 5:e5313b695302 124 if(revsec) spinTimer.attach(&fixedSpeed, spinWait);
david_s95 5:e5313b695302 125
david_s95 5:e5313b695302 126 }
david_s95 5:e5313b695302 127
david_s95 5:e5313b695302 128 void Rx_interrupt(void)
david_s95 5:e5313b695302 129 {
david_s95 5:e5313b695302 130 // NVIC_DisableIRQ(USB_IRQn);
david_s95 5:e5313b695302 131 HAL_NVIC_DisableIRQ(USB0_IRQn);
david_s95 5:e5313b695302 132 //// printf("Interrupt\n\r");
david_s95 5:e5313b695302 133 // pc.putc(48);
david_s95 5:e5313b695302 134 // pc.putc(pc.getc());
david_s95 5:e5313b695302 135 // printf("Lol");
david_s95 5:e5313b695302 136
david_s95 5:e5313b695302 137 char command[ARRAYSIZE];
david_s95 5:e5313b695302 138 int index=0;
david_s95 5:e5313b695302 139 char ch;
david_s95 5:e5313b695302 140 //
david_s95 5:e5313b695302 141 // //command[i++]=pc.getc();
david_s95 5:e5313b695302 142 // ch=pc.getc();
david_s95 5:e5313b695302 143
david_s95 5:e5313b695302 144 do {
david_s95 5:e5313b695302 145 if (pc.readable()) { // if there is a character to read from the device
david_s95 5:e5313b695302 146 ch = USB->RBR; // read it
david_s95 5:e5313b695302 147 if (index<ARRAYSIZE) command[index++]=ch; // put it into the value array and increment the index
david_s95 5:e5313b695302 148 }
david_s95 5:e5313b695302 149 } while (ch!='\n'); // loop until the \n character
david_s95 5:e5313b695302 150
david_s95 5:e5313b695302 151 // NVIC_EnableIRQ(USB_IRQn);
david_s95 5:e5313b695302 152 HAL_NVIC_EnableIRQ(USB_IRQn);
david_s95 5:e5313b695302 153
david_s95 5:e5313b695302 154 // command[index]='\x0'; // add un 0 to end the c string
david_s95 5:e5313b695302 155
david_s95 5:e5313b695302 156 int units = 0, tens = 0, decimals = 0;
david_s95 5:e5313b695302 157 switch (command[0]) {
david_s95 5:e5313b695302 158 case 'V':
david_s95 5:e5313b695302 159 //If decimal point is in the second character (eg, V.1)
david_s95 5:e5313b695302 160 if(command[1]=='.') {
david_s95 5:e5313b695302 161 //Extract decimal rev/s
david_s95 5:e5313b695302 162 decimals = command[2] - '0';
david_s95 5:e5313b695302 163 //If decimal point is in the third character (eg, V0.1)
david_s95 5:e5313b695302 164 } else if(command[2]=='.') {
david_s95 5:e5313b695302 165 units = command[1] - '0';
david_s95 5:e5313b695302 166 decimals = command[3] - '0';
david_s95 5:e5313b695302 167 //If decimal point is in the fourth character (eg, V10.1)
david_s95 5:e5313b695302 168 } else if(command[3]=='.') {
david_s95 5:e5313b695302 169 tens = command[1] - '0';
david_s95 5:e5313b695302 170 units = command[2] - '0';
david_s95 5:e5313b695302 171 decimals = command[4] - '0';
david_s95 5:e5313b695302 172 }
david_s95 5:e5313b695302 173 //Calculate the number of revolutions per second required
david_s95 5:e5313b695302 174 revsec = tens*10 + units + decimals/10;
david_s95 5:e5313b695302 175 //Calculate the required wait period
david_s95 5:e5313b695302 176 spinWait = (1/revsec)/6;
david_s95 5:e5313b695302 177 //Print values for verification
david_s95 5:e5313b695302 178 // pc.printf("Rev/S: %2.2f, Wait: %2.2f\n\r", revsec, spinWait);
david_s95 5:e5313b695302 179 break;
david_s95 5:e5313b695302 180 default:
david_s95 5:e5313b695302 181 // pc.printf("Error in received data\n\r");
david_s95 5:e5313b695302 182 break;
david_s95 5:e5313b695302 183 }
david_s95 5:e5313b695302 184 return;
david_s95 5:e5313b695302 185 }
mengkiang 4:f8a9ce214db9 186 //Main function
david_s95 5:e5313b695302 187 int main()
david_s95 5:e5313b695302 188 {
estott 0:de4320f74764 189 pc.printf("Hello\n\r");
david_s95 5:e5313b695302 190
david_s95 5:e5313b695302 191 // Setup a serial interrupt function to receive data
david_s95 5:e5313b695302 192 pc.attach(&Rx_interrupt, Serial::RxIrq);
david_s95 5:e5313b695302 193 // NVIC_EnableIRQ(USB_IRQn);
david_s95 5:e5313b695302 194 HAL_NVIC_EnableIRQ(USB_IRQn);
david_s95 5:e5313b695302 195 pc.putc('a');
estott 0:de4320f74764 196 //Run the motor synchronisation
estott 2:4e88faab6988 197 orState = motorHome();
estott 2:4e88faab6988 198 pc.printf("Rotor origin: %x\n\r",orState);
estott 2:4e88faab6988 199 //orState is subtracted from future rotor state inputs to align rotor and motor states
david_s95 5:e5313b695302 200 int counter = 0;
david_s95 5:e5313b695302 201
david_s95 5:e5313b695302 202 while(1) {
david_s95 5:e5313b695302 203 clk = !clk;
david_s95 5:e5313b695302 204 wait(0.5);
estott 2:4e88faab6988 205 }
david_s95 5:e5313b695302 206
david_s95 5:e5313b695302 207
estott 0:de4320f74764 208 }
estott 0:de4320f74764 209
david_s95 5:e5313b695302 210
david_s95 5:e5313b695302 211 //#include "mbed.h"
david_s95 5:e5313b695302 212 //#include "RawSerial.h"
david_s95 5:e5313b695302 213 //
david_s95 5:e5313b695302 214 //DigitalOut clk(LED1);
david_s95 5:e5313b695302 215 //DigitalOut dat(LED2);
david_s95 5:e5313b695302 216 //DigitalOut enable(LED3);
david_s95 5:e5313b695302 217 //DigitalOut bit(LED4);
david_s95 5:e5313b695302 218 //
david_s95 5:e5313b695302 219 //RawSerial pc(USBTX, USBRX);
david_s95 5:e5313b695302 220 //
david_s95 5:e5313b695302 221 //char ch;
david_s95 5:e5313b695302 222 //
david_s95 5:e5313b695302 223 //void flip(void) {
david_s95 5:e5313b695302 224 // clk = !clk;
david_s95 5:e5313b695302 225 // ch = LPC_UART0->RBR;
david_s95 5:e5313b695302 226 //
david_s95 5:e5313b695302 227 // LPC_UART0->RBR = ch;
david_s95 5:e5313b695302 228 //}
david_s95 5:e5313b695302 229 //
david_s95 5:e5313b695302 230 //int main() {
david_s95 5:e5313b695302 231 // clk = 1;
david_s95 5:e5313b695302 232 // pc.attach(&flip, Serial::RxIrq);
david_s95 5:e5313b695302 233 // while(1) {
david_s95 5:e5313b695302 234 // dat = !dat;
david_s95 5:e5313b695302 235 // wait(0.5);
david_s95 5:e5313b695302 236 // }
david_s95 5:e5313b695302 237 //}
david_s95 5:e5313b695302 238 //