Final version
Dependencies: Crypto_light mbed-rtos mbed
Fork of EMBEDDED_CW2_Final by
Diff: main.cpp
- Revision:
- 4:e322ca760c63
- Parent:
- 3:569b35e2a602
- Child:
- 5:e4b799086bc1
--- a/main.cpp Thu Mar 01 09:41:46 2018 +0000 +++ b/main.cpp Tue Mar 20 13:23:18 2018 +0000 @@ -1,4 +1,8 @@ #include "mbed.h" +#include "SHA256.h" +#include "rtos.h" +#include "slre.h" +#define char_len_max 32 //Photointerrupter input pins #define I1pin D2 @@ -7,7 +11,7 @@ //Incremental encoder input pins #define CHA D7 -#define CHB D8 +#define CHB D8 //Motor Drive output pins //Mask in output byte #define L1Lpin D4 //0x01 @@ -33,88 +37,516 @@ const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00}; //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid -const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07}; +const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07}; //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed //Phase lead to make motor spin -const int8_t lead = 2; //2 for forwards, -2 for backwards +int8_t lead = 2; //2 for forwards, -2 for backwards +int8_t oldLead = lead; +//Status LED -//Status LED DigitalOut led1(LED1); //Photointerrupter inputs -DigitalIn I1(I1pin); -DigitalIn I2(I2pin); -DigitalIn I3(I3pin); +InterruptIn I1(I1pin); +InterruptIn I2(I2pin); +InterruptIn I3(I3pin); //Motor Drive outputs -DigitalOut L1L(L1Lpin); +PwmOut L1L(L1Lpin); DigitalOut L1H(L1Hpin); -DigitalOut L2L(L2Lpin); +PwmOut L2L(L2Lpin); DigitalOut L2H(L2Hpin); -DigitalOut L3L(L3Lpin); +PwmOut L3L(L3Lpin); DigitalOut L3H(L3Hpin); -//Set a given drive state -void motorOut(int8_t driveState){ - - //Lookup the output byte from the drive state. - int8_t driveOut = driveTable[driveState & 0x07]; - - //Turn off first - if (~driveOut & 0x01) L1L = 0; - if (~driveOut & 0x02) L1H = 1; - if (~driveOut & 0x04) L2L = 0; - if (~driveOut & 0x08) L2H = 1; - if (~driveOut & 0x10) L3L = 0; - if (~driveOut & 0x20) L3H = 1; - - //Then turn on - if (driveOut & 0x01) L1L = 1; - if (driveOut & 0x02) L1H = 0; - if (driveOut & 0x04) L2L = 1; - if (driveOut & 0x08) L2H = 0; - if (driveOut & 0x10) L3L = 1; - if (driveOut & 0x20) L3H = 0; - } - - //Convert photointerrupter inputs to a rotor state -inline int8_t readRotorState(){ - return stateMap[I1 + 2*I2 + 4*I3]; +//Initialise the serial port +RawSerial pc(SERIAL_TX, SERIAL_RX); + +//***********Initialisation Our Variables************// + +//Message IDs +enum message_code { + ERROR_C = 0, //Error message ID + HASH = 1, //Hash frequency ID + NONCE = 2, //correct nonce ID + POSITION = 3, //Starting Rotor ID + DECODED = 4, //Decoded message ID + NEW_KEY = 5, + OLD_KEY = 6, + VELOCITY = 7, + NEW_SPEED = 8, + OLD_SPEED = 9, + NEW_TORQUE = 10, + NEW_ROTATIONS = 11 +}; + +//message structure +typedef struct{ + uint8_t code; //ID + uint64_t data; //Data + } message_t; + +Mail<message_t,16> outMessages; //Output message queue +Queue<void, 8> inCharQ; //character inputs + +int8_t orState; //starting state of the rotor +uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; +uint64_t* key = (uint64_t*)((int)sequence + 48); //Key generation +uint64_t* nonce = (uint64_t*)((int)sequence + 56); //Nonce +uint8_t hash[32]; //Hash output +char commInChar[char_len_max]; //array 32 characters length +uint8_t ptr; //char array pointer +volatile uint64_t newKey; //means value can change between thread calls +uint64_t oldKey; +Mutex newKey_mutex; //Stops the value from beng changed during use +int newSpeed = 30; +Mutex newSpeed_mutex; +uint32_t period = 2000; +uint32_t torqueVal = 1000; +uint32_t kp = 25; +uint32_t kd = 20; +int noRotations = 0; +bool dirSwitch = false; +bool rotate = false; +bool rotStart = false; + +Thread commOutT(osPriorityNormal,1024); //Output Thread +Thread commInT(osPriorityNormal,1024); //Input Thread +Thread motorCtrlT(osPriorityNormal,1024); + +void init_pwm(){ + L1L.period_us(period); + L2L.period_us(period); + L3L.period_us(period); +} + + +void putMessage(uint8_t code, uint64_t data){ + message_t *pMessage = outMessages.alloc(); //allocated the recieved message to outmessages + pMessage->code = code; + pMessage->data = data; + outMessages.put(pMessage); } -//Basic synchronisation routine -int8_t motorHome() { - //Put the motor in drive state 0 and wait for it to stabilise - motorOut(0); - wait(2.0); - - //Get the rotor state - return readRotorState(); +void commOutFn(){ + while(1){ + osEvent newEvent = outMessages.get(); //pulls the message + message_t *pMessage = (message_t*)newEvent.value.p; //assigns the values to pmessage + + switch(pMessage->code){ //finds correct ID for message + case ERROR_C: + if(pMessage->data == 0){ //Input message was too large + pc.printf("Input command too large\n\r"); + } + else if(pMessage->data == 1){ //Input message was too large + pc.printf("Key of wrong format\n\r"); + } + break; + case HASH: + pc.printf("Hash frequency %d Hz \n\r",pMessage->data); //outputs the hash frequency + break; + case NONCE: + pc.printf("Found a nonce 0x%016x\n\r", pMessage->data); //outputs correct nonce + break; + case POSITION: + pc.printf("Rotor Starting Position: %d\n\r", pMessage->data); //outputs starting position + break; + case DECODED: + if (pMessage->data == 0) { + pc.printf("Decoded as max speed\n\r"); + } + else if (pMessage->data == 1) { + pc.printf("Decoded no rotations\n\r"); + } + else if (pMessage->data == 2) { + pc.printf("Decoded key K\n\r"); + } + else if (pMessage->data == 3) { + pc.printf("Decoded torque T\n\r"); + } + break; + case NEW_KEY: + pc.printf("Decoded new key 0x%016llx\n\r",pMessage->data); + break; + case OLD_KEY: + pc.printf("Decoded new key same as old key: 0x%016llx\n\r",pMessage->data); + break; + case VELOCITY: + pc.printf("Current speed: %d\n\r",pMessage->data); + break; + case NEW_SPEED: + pc.printf("New speed: %d\n\r",pMessage->data); + break; + case OLD_SPEED: + pc.printf("New speed same as old speed: %d\n\r",pMessage->data); + break; + case NEW_TORQUE: + pc.printf("New torque: %d\n\r",pMessage->data); + break; + case NEW_ROTATIONS: + pc.printf("New number of rotations: %d\n\r",pMessage->data); + break; + } + outMessages.free(pMessage); //removes the message + } } - -//Main -int main() { - int8_t orState = 0; //Rotot offset at motor state 0 - int8_t intState = 0; - int8_t intStateOld = 0; - - //Initialise the serial port - Serial pc(SERIAL_TX, SERIAL_RX); - pc.printf("Hello\n\r"); - - //Run the motor synchronisation - orState = motorHome(); - pc.printf("Rotor origin: %x\n\r",orState); - //orState is subtracted from future rotor state inputs to align rotor and motor states - - //Poll the rotor state and set the motor outputs accordingly to spin the motor - while (1) { - intState = readRotorState(); - if (intState != intStateOld) { - intStateOld = intState; - motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive + +void serialISR(){ + uint8_t newChar = pc.getc(); //gets valuee from serial port + inCharQ.put((void*)newChar); //places into newChar + } + +void decode_char(char* buffer, uint8_t index){ + + struct slre regex; + struct cap captures[0 + 1]; + if(buffer[index] == 'V'){ //if first value is R rotate cretain number of times + putMessage(DECODED,0); + newSpeed_mutex.lock(); + sscanf(buffer, "V%d", &newSpeed); + if(newSpeed == 0){ + newSpeed = 120; + } + putMessage(NEW_SPEED,newSpeed); + newSpeed_mutex.unlock(); + } + else if(buffer[index] == 'v'){ //if first value is R rotate cretain number of times + putMessage(DECODED,0); + newSpeed_mutex.lock(); + sscanf(buffer, "v%d", &newSpeed); + if(newSpeed == 0){ + newSpeed = 120; + } + putMessage(NEW_SPEED,newSpeed); + newSpeed_mutex.unlock(); + + } + else if(buffer[index] == 'R'){ //if first value is V set speed of rotation + putMessage(DECODED,1); + sscanf(buffer, "R%ld", &noRotations); + rotate = true; + rotStart = true; + putMessage(NEW_ROTATIONS,noRotations); + } + else if(buffer[index] == 'r'){ //if first value is V set speed of rotation + putMessage(DECODED,1); + sscanf(buffer, "r%ld", &noRotations); + rotate = true; + rotStart = true; + putMessage(NEW_ROTATIONS,noRotations); + } + else if (buffer[index] == 'K'){ //if char is K set key to value input + putMessage(DECODED,2); + if(!slre_compile(®ex, "K[0-9a-fA-F]{16}")){ + putMessage(ERROR_C,1); + } + else if(slre_match(®ex, buffer, 16, captures)){ + newKey_mutex.lock(); + sscanf(buffer, "K%llx", &newKey); + if(oldKey != newKey){ + putMessage(NEW_KEY,newKey); + *key = newKey; + oldKey = newKey; + } + else{ + putMessage(OLD_KEY,oldKey); + } + newKey_mutex.unlock(); + } + else { + putMessage(ERROR_C,1); + } + } + else if (buffer[index] == 'k'){ //if char is K set key to value input + putMessage(DECODED,2); +// if(!slre_compile(®ex, "k[0-9a-fA-F]{16}")){ +// putMessage(ERROR_C,1); +// } +// else if(slre_match(®ex, buffer, char_len_max, captures)){ + newKey_mutex.lock(); + sscanf(buffer, "k%llx", &newKey); + if(oldKey != newKey){ + putMessage(NEW_KEY,newKey); + *key = newKey; + oldKey = newKey; + } + else{ + putMessage(OLD_KEY,oldKey); + } + newKey_mutex.unlock(); +// } +// else { +// putMessage(ERROR_C,1); +// } + } + else if (buffer[index] == 'p'){ //if char is K set key to value inpu + sscanf(buffer, "p%lld", &kp); + putMessage(NEW_TORQUE,kp); + } +} + +void commInFn(){ + pc.printf("Enter your command:\n\r"); //Tells the person to input their message + pc.attach(&serialISR); //looks for the serialISR to get message + while(1){ + if(ptr >= char_len_max){ + putMessage(ERROR_C,0); //if gone over the buffer length, cancel and restart for next input + ptr = 0; //reset pointer + break; + } + osEvent newEvent = inCharQ.get(); //get next character + uint8_t newChar = (uint8_t)newEvent.value.p; + if(newChar != '\r' && newChar != '\n'){ + commInChar[ptr] = newChar; //place values into buffer + ptr++; //increment pointer + } + else{ + commInChar[ptr] = '\0'; //defines the end of the command + ptr = 0; //resets the pointer + decode_char(commInChar,ptr); //sends array to decoding function } } } +//Set a given drive state +void motorOut(int8_t driveState, uint32_t torque){ + + //Lookup the output byte from the drive state. + int8_t driveOut = driveTable[driveState & 0x07]; + + //Turn off first + if (~driveOut & 0x01) L1L.pulsewidth_us(0); + if (~driveOut & 0x02) L1H = 1; + if (~driveOut & 0x04) L2L.pulsewidth_us(0); + if (~driveOut & 0x08) L2H = 1; + if (~driveOut & 0x10) L3L.pulsewidth_us(0); + if (~driveOut & 0x20) L3H = 1; + + //Then turn on + if (driveOut & 0x01) L1L.pulsewidth_us(torque); + if (driveOut & 0x02) L1H = 0; + if (driveOut & 0x04) L2L.pulsewidth_us(torque); + if (driveOut & 0x08) L2H = 0; + if (driveOut & 0x10) L3L.pulsewidth_us(torque); + if (driveOut & 0x20) L3H = 0; +} + +//Convert photointerrupter inputs to a rotor state +inline int8_t readRotorState(){ + return stateMap[I1 + 2*I2 + 4*I3]; +} + +//Basic synchronisation routine +int8_t motorHome(){ + //Put the motor in drive state 0 and wait for it to stabilise + motorOut(0,torqueVal); + wait(1.0); + lead = 0; + + //Get the rotor state + return readRotorState(); +} + +int32_t motorPosition; +void motorISR(){ + static int8_t oldRotorState; + int8_t rotorState = readRotorState(); //reads motor position + motorOut((rotorState-orState+lead+6)%6,torqueVal); //+6 to make sure the remainder is positive + if(rotorState - orState==5) motorPosition--; + else if(rotorState - orState== -5) motorPosition++; + else motorPosition+=(rotorState - orState); + oldRotorState = rotorState; +} + +void motorCtrlTick(){ + motorCtrlT.signal_set(0x1); +} + +Timer t_motor; +void motorCtrlFn(){ + + float v, v_avg, ys, yr, dEr; + int i = 0, dt, oldPosition, totPosition, position, startPosition, newEr, oldEr, mainLead; + t_motor.start(); + Ticker motorCtrlTicker; + motorCtrlTicker.attach_us(&motorCtrlTick,100000); + while(1){ + if(rotate){ + if(rotStart){ + if(noRotations > 0){ + lead = 2; + } + else if(noRotations < 0){ + lead = -2; + } + else if(noRotations == 0 && lead == 0){ + lead = -2; + } + i = 0; + v_avg = 0; + mainLead = lead; //sets general direction + totPosition = (int)6*noRotations; //nimber of position changes required + oldEr = totPosition; //how far away + rotStart = false; //stops from running this loop + motorCtrlT.signal_wait(0x1); //waits for tick + __disable_irq(); //disables interrupts + startPosition = motorPosition; //sets start position at present motor position + oldPosition = startPosition; //sets old position to same value + t_motor.reset(); + motorPosition = 0; //resets time and motorPosition + __enable_irq(); //enables interrupts + } + else if(noRotations == 0){//if to spin forever + i++; //increment counter + motorCtrlT.signal_wait(0x1); + __disable_irq(); + position = motorPosition; //adds on number of rotations + dt = t_motor.read_ms(); //change in time + t_motor.reset(); //resets time + motorPosition = 0; //resets motor position + __enable_irq(); //enables interrupts + v = 1000.0*(((float)position)/(float)dt)/6.0; //calculates velocity + v_avg += v; //adds speed onto averager + if((int)abs(v) < 4 && newSpeed != 0){ + lead = mainLead; //makes sure it's in the correct direction + torqueVal = 1000; //sets torque + motorISR(); //moves the motor + } + ys = kp*(newSpeed-abs(v)); //speed controller + if(ys < 0){ + lead = mainLead*-1; + } + else{ + lead = mainLead; + } + torqueVal = abs(ys); + if(torqueVal > 1000){ + torqueVal = 1000; + } + } + else{ + i++; //increment counter + motorCtrlT.signal_wait(0x1); + __disable_irq(); + position += motorPosition; //adds on number of rotations + dt = t_motor.read_ms(); //change in time + t_motor.reset(); //resets time + motorPosition = 0; //resets motor position + __enable_irq(); //enables interrupts + v = 1000.0*(((float)position-(float)oldPosition)*(lead/2)/(float)dt)/6.0; //calculates velocity + oldPosition = position; //changes old position + newEr = totPosition+(position)*(lead/2); //difference in placement + dEr = (newEr-oldEr)/dt; //change against time + oldEr = newEr; //old is same as new + yr = kp*newEr + kd*dEr; //rotational controller + v_avg += v; //adds speed onto averager + newSpeed_mutex.lock(); //locks newSpeed + ys = kp*(newSpeed-abs(v))*(newEr/abs(newEr)); //speed controller + pc.printf("%d\r\n",newEr); + if(abs(newEr) < 6){ + lead = 0; +// pc.printf("%f\r\n",dEr); + } + else if(((int)abs(v) < 4) && (newSpeed != 0)){ + lead = mainLead; //makes sure it's in the correct direction + torqueVal = 1000; //sets torque + motorISR(); //moves the motor + } + else if(v < 0){ //if speed is negative + if(ys > yr){ //take the largest value + torqueVal = abs(ys); + pc.printf(" + if(ys < 0){ + lead = lead*-1; //reverse direction + } + else{ + lead = mainLead; //set to correct direction + } + } + else{ + torqueVal = abs(yr); //set torque + if(ys < 0){ + lead = lead*-1; + } + else{ + lead = mainLead; + } + } + } + else{ + if(ys < yr){ //if v is positive select smallest + torqueVal = abs(ys); + if(ys < 0){ + lead = lead*-1; + } + else{ + lead = mainLead; + } + } + else{ + torqueVal = abs(yr); + if(yr < 0){ + lead = lead*-1; + } + else{ + lead = mainLead; + } + } + } + if(torqueVal > 1000){ + torqueVal = 1000; + } + + } + newSpeed_mutex.unlock(); + if (i==10){ + v_avg = v_avg/i; + putMessage(VELOCITY, (uint64_t)abs(v_avg)); + v_avg = 0; + i= 0; + } + } + } +} +//Main +int main(){ + pc.printf("Hello\n\r"); //outputs hello when turned on + init_pwm(); + commOutT.start(commOutFn); //starts the output and input threads + commInT.start(commInFn); + //Run the motor synchronisation + orState = motorHome(); //finds staring position + putMessage(POSITION,orState); + + Timer t; //adds a timer to count number of hashes per second + + //orState is subtracted from future rotor state inputs to align rotor and motor states + //Poll the rotor state and set the motor outputs accordingly to spin the motor + I1.rise(&motorISR); //looks for rising edge to trigger the motor change + I2.rise(&motorISR); + I3.rise(&motorISR); + I1.fall(&motorISR); //looks for rising edge to trigger the motor change + I2.fall(&motorISR); + I3.fall(&motorISR); + uint16_t counter; + counter = 0; //initialised and set to 0 to count number of hashes + t.start(); //starts the timer + motorCtrlT.start(motorCtrlFn); + while (1) { + + if(t.read_ms() >= 1000){ //if more than 1 second has surpased +// putMessage(HASH, counter); //outputs the hash frequency + counter = 0; //reset counter + t.reset(); //resets the timer + } + SHA256::computeHash(&hash[0],&sequence[0],sizeof(sequence)); //computes the hash + counter++; //increments counter; + + if((hash[0] == 0) && (hash[1] == 0)){ +// putMessage(NONCE,*nonce); //when hash is correct print the nonce + } + + *nonce += 1; //increments nonce + } +}