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

diff -r e322ca760c63 -r e4b799086bc1 main.cpp
--- 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(&regex, "K[0-9a-fA-F]{16}")){
-            putMessage(ERROR_C,1);
-        }
-        else if(slre_match(&regex, 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(&regex, "K[0-9a-fA-F]{16}")) {
+            putMessage(ERROR_C,(uint64_t)1);
+        } else if(slre_match(&regex, 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(&regex, "k[0-9a-fA-F]{16}")){
 //            putMessage(ERROR_C,1);
 //        }
 //        else if(slre_match(&regex, 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