Jacob Kay
Final
Dependencies: Crypto_light mbed-rtos mbed regex
Fork of
EMBEDDED_CW2
by 
George Padley
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
+ }
+}