![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Final
Dependencies: Crypto_light mbed-rtos mbed regex
Fork of EMBEDDED_CW2 by
Revision 5:e4b799086bc1, committed 2018-03-23
- Comitter:
- JacobKay97
- Date:
- Fri Mar 23 15:42:42 2018 +0000
- Parent:
- 4:e322ca760c63
- Commit message:
- Final
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Mar 20 13:23:18 2018 +0000 +++ b/main.cpp Fri Mar 23 15:42:42 2018 +0000 @@ -82,10 +82,13 @@ }; //message structure -typedef struct{ +typedef struct { uint8_t code; //ID uint64_t data; //Data - } message_t; + float dataf; //Fudged it +} message_t; + + Mail<message_t,16> outMessages; //Output message queue Queue<void, 8> inCharQ; //character inputs @@ -100,51 +103,62 @@ 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; +float newSpeed = 30.0f; Mutex newSpeed_mutex; uint32_t period = 2000; uint32_t torqueVal = 1000; -uint32_t kp = 25; -uint32_t kd = 20; -int noRotations = 0; +int32_t kp = 25; +int32_t kp2 = 25; +int32_t kd = 20; //Why aren't we using a define?? +float noRotations = 0.0f; bool dirSwitch = false; bool rotate = false; bool rotStart = false; Thread commOutT(osPriorityNormal,1024); //Output Thread -Thread commInT(osPriorityNormal,1024); //Input Thread +Thread commInT(osPriorityNormal,1200); //Input Thread Thread motorCtrlT(osPriorityNormal,1024); -void init_pwm(){ +void init_pwm() +{ L1L.period_us(period); L2L.period_us(period); L3L.period_us(period); } -void putMessage(uint8_t code, uint64_t data){ +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 commOutFn(){ - while(1){ +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 + switch(pMessage->code) { //finds correct ID for message case ERROR_C: - if(pMessage->data == 0){ //Input message was too large + 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 + } 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 + 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 @@ -155,14 +169,11 @@ case DECODED: if (pMessage->data == 0) { pc.printf("Decoded as max speed\n\r"); - } - else if (pMessage->data == 1) { + } else if (pMessage->data == 1) { pc.printf("Decoded no rotations\n\r"); - } - else if (pMessage->data == 2) { + } else if (pMessage->data == 2) { pc.printf("Decoded key K\n\r"); - } - else if (pMessage->data == 3) { + } else if (pMessage->data == 3) { pc.printf("Decoded torque T\n\r"); } break; @@ -173,10 +184,10 @@ 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); + pc.printf("Current speed: %f\n\r",pMessage->dataf); break; case NEW_SPEED: - pc.printf("New speed: %d\n\r",pMessage->data); + pc.printf("New speed: %f\n\r",pMessage->dataf); break; case OLD_SPEED: pc.printf("New speed same as old speed: %d\n\r",pMessage->data); @@ -185,124 +196,121 @@ pc.printf("New torque: %d\n\r",pMessage->data); break; case NEW_ROTATIONS: - pc.printf("New number of rotations: %d\n\r",pMessage->data); + pc.printf("New number of rotations: %f\n\r",pMessage->dataf); break; } outMessages.free(pMessage); //removes the message } } -void serialISR(){ +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){ +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); + 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%d", &newSpeed); - if(newSpeed == 0){ - newSpeed = 120; + 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,0); + } 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%d", &newSpeed); - if(newSpeed == 0){ - newSpeed = 120; + 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,1); - sscanf(buffer, "R%ld", &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] == 'r'){ //if first value is V set speed of rotation - putMessage(DECODED,1); - sscanf(buffer, "r%ld", &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 - 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)){ + } else if (buffer[index] == 'K') { //if char is K set key to value input + putMessage(DECODED,(uint64_t)2); + if(!slre_compile(®ex, "K[0-9a-fA-F]{16}")) { + putMessage(ERROR_C,(uint64_t)1); + } else if(slre_match(®ex, buffer, 16, captures)) { newKey_mutex.lock(); sscanf(buffer, "K%llx", &newKey); - if(oldKey != newKey){ + if(oldKey != newKey) { putMessage(NEW_KEY,newKey); *key = newKey; oldKey = newKey; - } - else{ + } else { putMessage(OLD_KEY,oldKey); } newKey_mutex.unlock(); - } - else { - putMessage(ERROR_C,1); + } else { + putMessage(ERROR_C,(uint64_t)1); } - } - else if (buffer[index] == 'k'){ //if char is K set key to value input - putMessage(DECODED,2); + } else if (buffer[index] == 'k') { //if char is K set key to value input + putMessage(DECODED,(uint64_t)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(); + 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 + } else if (buffer[index] == 'p') { //if char is K set key to value inpu sscanf(buffer, "p%lld", &kp); - putMessage(NEW_TORQUE,kp); + putMessage(NEW_TORQUE,(uint64_t)kp); } } -void commInFn(){ +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 + 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 break; } osEvent newEvent = inCharQ.get(); //get next character uint8_t newChar = (uint8_t)newEvent.value.p; - if(newChar != '\r' && newChar != '\n'){ + 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 + } 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 } @@ -310,7 +318,8 @@ } //Set a given drive state -void motorOut(int8_t driveState, uint32_t torque){ +void motorOut(int8_t driveState, uint32_t torque) +{ //Lookup the output byte from the drive state. int8_t driveOut = driveTable[driveState & 0x07]; @@ -333,14 +342,17 @@ } //Convert photointerrupter inputs to a rotor state -inline int8_t readRotorState(){ +inline int8_t readRotorState() +{ return stateMap[I1 + 2*I2 + 4*I3]; } //Basic synchronisation routine -int8_t motorHome(){ +int8_t motorHome() +{ //Put the motor in drive state 0 and wait for it to stabilise - motorOut(0,torqueVal); + //motorOut(0,torqueVal); + motorOut(0,1000); wait(1.0); lead = 0; @@ -349,178 +361,182 @@ } int32_t motorPosition; -void motorISR(){ +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); + if(rotorState - oldRotorState==5) motorPosition--; + else if(rotorState - oldRotorState== -5) motorPosition++; + else motorPosition+=(rotorState - oldRotorState); oldRotorState = rotorState; } -void motorCtrlTick(){ +void motorCtrlTick() +{ motorCtrlT.signal_set(0x1); } Timer t_motor; -void motorCtrlFn(){ +void motorCtrlFn() +{ float v, v_avg, ys, yr, dEr; int i = 0, dt, oldPosition, totPosition, position, startPosition, newEr, oldEr, mainLead; + bool jacobFudge = false; t_motor.start(); Ticker motorCtrlTicker; motorCtrlTicker.attach_us(&motorCtrlTick,100000); - while(1){ - if(rotate){ - if(rotStart){ - if(noRotations > 0){ + while(1) { + motorCtrlT.signal_wait(0x1); + if(rotate) { + if(rotStart) { + if(noRotations > 0) { lead = 2; - } - else if(noRotations < 0){ + } else if(noRotations < 0) { lead = -2; - } - else if(noRotations == 0 && lead == 0){ - lead = -2; + } else if(noRotations == 0 && lead == 0) { + lead = 2; } i = 0; v_avg = 0; + yr = 0.0f; + ys = 0.0f; + dEr = 0.0f; 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 + position = 0; + jacobFudge = true; + + } 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 = (166.67f*((float)position/(float)dt)); //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)){ + 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 } - else if(v < 0){ //if speed is negative - if(ys > yr){ //take the largest value + newSpeed_mutex.unlock(); + ys = kp*(newSpeed-abs(v)); //speed controller + if(ys < 0) { + lead = mainLead*-1; + } else { + lead = mainLead; + } + if(abs(ys) > 1000) { + torqueVal = 1000; + } else { + torqueVal = abs(ys); + } + // 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)kp2*(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(jacobFudge == true) { + lead = mainLead; //makes sure it's in the correct direction + torqueVal = 900; //sets torque + motorISR(); //moves the motor + jacobFudge = false; + + } + if(v >=0.0f) { //if speed is +ve + if(abs(ys)<abs(yr)) { torqueVal = abs(ys); - pc.printf(" - if(ys < 0){ - lead = lead*-1; //reverse direction - } - else{ - lead = mainLead; //set to correct direction - } + } else { + torqueVal = abs(yr); + } + if(abs(newEr) <=5) { + torqueVal = 0; + lead = 0; + rotate = false; + pc.printf("NewErr %d\r\n",newEr); + } else if (yr<-500 && dEr <0) + torqueVal = abs(yr); + lead = -2; + } else { + lead = 2; } - else{ - torqueVal = abs(yr); //set torque - if(ys < 0){ - lead = lead*-1; - } - else{ - lead = mainLead; - } + } else { + if(abs(ys)>abs(yr)) { + torqueVal = abs(ys); + } else { + torqueVal = abs(yr); + } + if(abs(newEr) <=5) { + torqueVal = 0; + lead = 0; + rotate = false; + pc.printf("NewErr %d\r\n",newEr); + } else if (yr>500 && dEr >0) + torqueVal = abs(yr); + lead = 2; + } else { + lead = -2; } } - 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 != 0 && lead !=0 && abs(v)== 0) { + torqueVal = torqueVal + 50; + motorISR(); + } + if(torqueVal > 1000) { + torqueVal = 1000; } - 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; - } + } + if (i==10) { + v_avg = v_avg/i; + putMessage(VELOCITY, v_avg); + v_avg = 0; + i= 0; } } } + + //Main -int 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); + 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 @@ -535,16 +551,16 @@ motorCtrlT.start(motorCtrlFn); while (1) { - if(t.read_ms() >= 1000){ //if more than 1 second has surpased -// putMessage(HASH, counter); //outputs the hash frequency + 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 + if((hash[0] == 0) && (hash[1] == 0)) { + putMessage(NONCE,*nonce); //when hash is correct print the nonce } *nonce += 1; //increments nonce