Final version
Dependencies: Crypto_light mbed-rtos mbed
Fork of EMBEDDED_CW2_Final by
main.cpp
- Committer:
- GPadley
- Date:
- 2018-03-23
- Revision:
- 6:96383d87c51f
- Parent:
- 5:e4b799086bc1
File content as of revision 6:96383d87c51f:
#include "mbed.h" #include "SHA256.h" #include "rtos.h" #define char_len_max 32 //Photointerrupter input pins #define I1pin D2 #define I2pin D11 #define I3pin D12 //Incremental encoder input pins #define CHA D7 #define CHB D8 //Motor Drive output pins //Mask in output byte #define L1Lpin D4 //0x01 #define L1Hpin D5 //0x02 #define L2Lpin D3 //0x04 #define L2Hpin D6 //0x08 #define L3Lpin D9 //0x10 #define L3Hpin D10 //0x20 //Mapping from sequential drive states to motor phase outputs /* State L1 L2 L3 0 H - L 1 - H L 2 L H - 3 L - H 4 - L H 5 H L - 6 - - - 7 - - - */ //Drive state to output table 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}; //Phase lead to make motor spin int8_t lead = 2; //2 for forwards, -2 for backwards int8_t oldLead = lead; //Status LED DigitalOut led1(LED1); //Photointerrupter inputs InterruptIn I1(I1pin); InterruptIn I2(I2pin); InterruptIn I3(I3pin); //Motor Drive outputs PwmOut L1L(L1Lpin); DigitalOut L1H(L1Hpin); PwmOut L2L(L2Lpin); DigitalOut L2H(L2Hpin); PwmOut L3L(L3Lpin); DigitalOut L3H(L3Hpin); //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, //new key ID OLD_KEY = 6, //old key ID VELOCITY = 7, //velocity NEW_SPEED = 8, NEW_ROTATIONS = 9 }; //message structure typedef struct { uint8_t code; //ID uint64_t data; //Data float dataf; //Fudged it } 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 float newSpeed = 30.0f; Mutex newSpeed_mutex; uint32_t torqueVal = 1000; float noRotations = 0.0f; bool dirSwitch = false; bool rotate = false; bool rotStart = false; int period = 2000; int kp = 25.0f; int kd = 20.0f; Thread commOutT(osPriorityNormal,1024); //Output Thread Thread commInT(osPriorityNormal,1200); //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); } void putMessage(uint8_t code, float data) { message_t *pMessage = outMessages.alloc(); //allocated the recieved message to outmessages pMessage->code = code; pMessage->dataf = data; outMessages.put(pMessage); } 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"); } break; case HASH: pc.printf("Hash Rate %d Hashes/sec \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: %f\n\r",pMessage->dataf); break; case NEW_SPEED: pc.printf("New speed: %f\n\r",pMessage->dataf); break; case NEW_ROTATIONS: pc.printf("New number of rotations: %f\n\r",pMessage->dataf); break; } outMessages.free(pMessage); //removes the message } } 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) { if(buffer[index] == 'V') { //if first value is R rotate cretain number of times putMessage(DECODED,(uint64_t)0); newSpeed_mutex.lock(); sscanf(buffer, "V%f", &newSpeed); if(newSpeed == 0.0f) { newSpeed = 120.0f; } else if(newSpeed < 0.0f) { newSpeed = fabsf(newSpeed); } putMessage(NEW_SPEED,newSpeed); newSpeed_mutex.unlock(); } else if(buffer[index] == 'v') { //if first value is R rotate cretain number of times putMessage(DECODED,(uint64_t)0); newSpeed_mutex.lock(); sscanf(buffer, "v%f", &newSpeed); if(newSpeed == 0.0f) { newSpeed = 120.0f; } else if(newSpeed < 0.0f) { newSpeed = fabsf(newSpeed); } putMessage(NEW_SPEED,newSpeed); newSpeed_mutex.unlock(); } else if(buffer[index] == 'R') { //if first value is V set speed of rotation putMessage(DECODED,(uint64_t)1); sscanf(buffer, "R%f", &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,(uint64_t)1); sscanf(buffer, "r%f", &noRotations); rotate = true; rotStart = true; putMessage(NEW_ROTATIONS,noRotations); } else if (buffer[index] == 'K') { //if char is K set key to value input 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 if (buffer[index] == 'k') { //if char is K set key to value input putMessage(DECODED,(uint64_t)2); 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(); } } 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,(uint64_t)0); //if gone over the buffer length, cancel and restart for next input ptr = 0; //reset pointer } 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 commInChar[ptr] = ' '; //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); //motor torque output 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); motorOut(0,1000); wait(1.0); //waits for stabalisation lead = 0; //stops rotation //Get the rotor state return readRotorState(); } int32_t motorPosition; void motorISR() { static int8_t oldRotorState; //remembers old state int8_t rotorState = readRotorState(); //reads motor position motorOut((rotorState-orState+lead+6)%6,torqueVal); //+6 to make sure the remainder is positive if(rotorState - oldRotorState==5) motorPosition--; //reverse else if(rotorState - oldRotorState== -5) motorPosition++; //forward else motorPosition+=(rotorState - oldRotorState); oldRotorState = rotorState;//remember previous state } void motorCtrlTick() { motorCtrlT.signal_set(0x1); //wait for 100 ms to send signal } Timer t_motor; void motorCtrlFn() { float v, v_avg, ys, yr, dEr; //local variables used int i = 0, dt, oldPosition, totPosition, position, startPosition, newEr, oldEr, mainLead; bool jumpStart = false; t_motor.start(); Ticker motorCtrlTicker; motorCtrlTicker.attach_us(&motorCtrlTick,100000); while(1) { motorCtrlT.signal_wait(0x1); if(rotate) { if(rotStart) { if(noRotations > 0) { lead = 2;//positive rotations } else if(noRotations < 0) { lead = -2;//negative rotations } else if(noRotations == 0 && lead == 0) { lead = 2;//if no direction specified spin forwards } i = 0; //reset variables v_avg = 0; yr = 0.0f; ys = 0.0f; dEr = 0.0f; mainLead = lead; //sets general direction totPosition = (int)6*noRotations; //number of position changes required oldEr = totPosition; //how far away rotStart = false; //stops from running this loop __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 position = 0; jumpStart = true; } else if(noRotations == 0) { //if to spin forever i++; //increment counter __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 = (166.67f*((float)position/(float)dt)); //calculates velocity v_avg += v; //adds speed onto averager newSpeed_mutex.lock(); if((int)abs(v) < 1 && newSpeed != 0) { lead = mainLead; //makes sure it's in the correct direction torqueVal = 1000; //sets torque motorISR(); //moves the motor } newSpeed_mutex.unlock(); ys = kp*(newSpeed-abs(v)); //speed controller if(ys < 0) { lead = mainLead*-1; //reverses direction } else { lead = mainLead; //goes forwards } if(abs(ys) > 1000) { torqueVal = 1000; //maximum speed } else { torqueVal = abs(ys); //makes sure absolute } // pc.printf("torque = %d\r\n",torqueVal); } else { i++; //increment counter __disable_irq(); position += motorPosition; //adds on number of rotations dt = t_motor.read_ms(); //change in time // pc.printf("motorPosPre = %d\r\n",motorPosition); t_motor.reset(); //resets time motorPosition = 0; //resets motor position __enable_irq(); //enables interrupts // pc.printf("Pos = %d\r\n",position); v = 166.67f*(((float)position-(float)oldPosition)/(float)dt); //calculates velocity oldPosition = position; //changes old position newEr = totPosition-position; //difference in placement dEr = 1000.0f*((float)newEr-(float)oldEr)/(float)dt; //change against time oldEr = newEr; //old is same as new yr = (float)kp*(float)newEr + (float)kd*dEr; //rotational controller v_avg += v; //adds speed onto averager ys = (float)kp*((float)newSpeed-fabsf(v))*((newEr > 0) ? 1.0f : ((newEr < 0) ? -1.0f : 0.0f)); //speed controller if(jumpStart == true) { lead = mainLead; //makes sure it's in the correct direction torqueVal = 900; //sets torque motorISR(); //moves the motor jumpStart = false; } if(v >=0.0f) { //if speed is +ve if(abs(ys)<abs(yr)) { torqueVal = abs(ys); //sets the torque output } else { torqueVal = abs(yr);//sets the torque output } if(abs(newEr) <=5) {//stops it spinning torqueVal = 0; lead = 0; rotate = false; pc.printf("NewErr %d\r\n",newEr); } else if (yr<-500 && dEr <0) { torqueVal = abs(yr);//go backwards lead = -2; } else { lead = 2; //go forwards } } else { if(abs(ys)>abs(yr)) { torqueVal = abs(ys); //take the highest value for torque } else { torqueVal = abs(yr); } if(abs(newEr) <=5) { //stop spinning when error loess than 1 rotation torqueVal = 0; lead = 0; rotate = false; } else if (yr>500 && dEr >0) { //goes backwards if overshot torqueVal = abs(yr); lead = 2; } else { lead = -2; } } if(torqueVal != 0 && lead !=0 && abs(v)== 0) { //makes sure motor spins if it is meant to torqueVal = torqueVal + 50; motorISR(); } if(torqueVal > 1000) { //stops non linear torque torqueVal = 1000; } } } if (i==10) {//every 1 second print velocity v_avg = v_avg/i; putMessage(VELOCITY, 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,(uint64_t)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, (uint64_t)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 } }