Revision 11:aae7c9c290e2, committed 2019-03-22

Comitter:

OmarMuttawa

Date:

Fri Mar 22 22:27:48 2019 +0000

Parent:

10:a4b5723b6c9d

Child:

12:b39f69ed20af

Commit message:

Final

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Crypto.lib	Fri Mar 22 22:27:48 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/feb11/code/Crypto/#f04410cef037

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Rawserial.bld	Fri Mar 22 22:27:48 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

--- a/main.cpp	Tue Feb 26 12:23:17 2019 +0000
+++ b/main.cpp	Fri Mar 22 22:27:48 2019 +0000
@@ -1,4 +1,8 @@
 #include "mbed.h"
+#include "SHA256.h"
+#include <string>
+#include <vector>
+#include <RawSerial.h>
 
 //Photointerrupter input pins
 #define I1pin D3
@@ -9,12 +13,12 @@
 #define CHApin   D12
 #define CHBpin   D11
 
-//Motor Drive output pins   //Mask in output byte
-#define L1Lpin D1           //0x01
-#define L1Hpin A3           //0x02
-#define L2Lpin D0           //0x04
+//Motor Drive output pins  //Mask in output byte
+#define L1Lpin D1          //0x01
+#define L1Hpin A3          //0x02
+#define L2Lpin D0          //0x04
 #define L2Hpin A6          //0x08
-#define L3Lpin D10           //0x10
+#define L3Lpin D10         //0x10
 #define L3Hpin D2          //0x20
 
 #define PWMpin D9
@@ -23,6 +27,8 @@
 #define MCSPpin   A1
 #define MCSNpin   A0
 
+#define TESTPIN D4  //USED FOR TIMING ETC
+
 //Mapping from sequential drive states to motor phase outputs
 /*
 State   L1  L2  L3
@@ -43,15 +49,47 @@
 //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
+volatile int8_t lead = 2;  //2 for forwards, -2 for backwards
 
 //Status LED
 DigitalOut led1(LED1);
+DigitalOut testPin(TESTPIN);
 
 //Photointerrupter inputs
-DigitalIn I1(I1pin);
-DigitalIn I2(I2pin);
-DigitalIn I3(I3pin);
+InterruptIn I1(I1pin);
+InterruptIn I2(I2pin);
+InterruptIn I3(I3pin);
+
+//Global varibale to count revolutions up to 6
+volatile int count_revolution = 0;
+volatile int number_of_notes = 0;
+volatile int last_count_revolution = 0;
+//TUNING PARAMETERS FOR SPEED CONTROLLER 
+volatile float K_PS = 0.1;
+volatile float K_IS = 0.18;
+volatile float K_DS = 0.0001;
+volatile float VELOCITY_INTEGRAL_LIMIT = 100;
+//TUNING PARAMETERS FOR REVOLUTION CONTROLLER 
+volatile float K_PR= 0.1;
+volatile float K_IR = 0.001;
+volatile float K_DR = 0.05;
+volatile float ROTATION_INTEGRAL_LIMIT = 100;
+
+float timeSinceLastRun;
+float elapsedTime = 0;
+
+string tune;
+char tune_c_str[50];
+
+char notes [50];        //hold the notes
+float periods[50];
+int durations[50];      //holds the durations
+volatile bool playTune = false;
+bool playTuneLocal = false; //used to prevent deadlocks and long mutexs
+int noteIndex = 0;
+
+//PWM Class:
+PwmOut PWM_pin(PWMpin);
 
 //Motor Drive outputs
 DigitalOut L1L(L1Lpin);
@@ -61,6 +99,70 @@
 DigitalOut L3L(L3Lpin);
 DigitalOut L3H(L3Hpin);
 
+/* Mail */
+typedef struct {
+  int type; //0 means it will be a string, 1 means it will be uint64
+  uint64_t uint64_message;
+  char* string_message;
+  float float_message;
+} mail_t;
+
+//Declaration of timer global variable
+Timer t;
+
+//Declaration of timer for velocity calculations global variable
+Timer motorCtrlTimer;
+
+//Threads' declaration
+Thread printMsgT;
+Thread readMsgT;
+Thread motorCtrlT(osPriorityHigh,1024);
+
+//Declaration of Mail global object
+Mail<mail_t, 16> mail_box;
+
+//Global to keep to previous position necessary for velocity calculations
+float prevPosition;
+
+//Global for motor's position
+float motor_position = 0;
+float current_position = 0;
+
+//Global variables
+int8_t intState,intStateOld,orState;
+
+RawSerial pc(SERIAL_TX, SERIAL_RX);
+Queue<void, 8> inCharQ;
+uint64_t newKey;
+float Ts,Tr,newTorque;
+volatile float rot_input=0;
+volatile float max_vel = 10;
+volatile int exec_count = 0;
+            
+Mutex newKey_mutex;
+Mutex newRotation_mutex;
+Mutex newVelocity_mutex;
+Mutex countRev_mutex;
+Mutex playTune_mutex;
+int hashRate;
+
+volatile float velocityError = 0;
+volatile float prevVelocityError=0;
+volatile float differentialVelocityError = 0;
+volatile float integralVelocityError = 0;
+
+volatile float rotationError = 0;
+volatile float prevRotationError = 0;
+volatile float differentialRotationError = 0;
+volatile float integralRotationError = 0;
+
+float max_t = 0;
+
+int target_position,position_error;
+
+//Global variable for velocity
+volatile float velocity;
+
 //Set a given drive state
 void motorOut(int8_t driveState){
     
@@ -84,44 +186,527 @@
     if (driveOut & 0x20) L3H = 0;
     }
     
-    //Convert photointerrupter inputs to a rotor state
+//Convert photointerrupter inputs to a rotor state
 inline int8_t readRotorState(){
     return stateMap[I1 + 2*I2 + 4*I3];
-    }
+}
 
+void Rotorcalib(){
+    intState = readRotorState();
+    if (intState != intStateOld) {
+        //lead_mutex.lock();
+        motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
+        //lead_mutex.unlock();
+        //pc.printf("%d\n\r",intState);
+        if (intState-intStateOld==-5){
+            //Goes positive direction
+            motor_position++;
+        }else if (intState-intStateOld==5) {
+            //Goes negative direction
+            motor_position--;
+        }else{
+            motor_position = motor_position + (intState-intStateOld);   //every 
+        }
+        //Update state
+        intStateOld = intState;
+    }    
+}
+    
 //Basic synchronisation routine    
 int8_t motorHome() {
+
     //Put the motor in drive state 0 and wait for it to stabilise
     motorOut(0);
     wait(2.0);
+    motorOut(1.0);
     
     //Get the rotor state
     return readRotorState();
 }
-    
+
+//Print message from Mail_box
+void printMsgFn (void) {
+    while(1){
+        
+        //Recieve a message and print it
+        osEvent evt = mail_box.get();
+        
+        if (evt.status == osEventMail) {
+            mail_t *mail = (mail_t*)evt.value.p;
+            if(mail->type == 1){    //int
+                printf("%llu\n\r", mail->uint64_message);
+            }
+            else if(mail->type == 2){
+                //string!
+                printf("%s", mail->string_message);
+            }else if (mail->type==3){
+                printf("%f\n\r", mail->float_message);
+            }
+            mail_box.free(mail);
+        }
+    }
+}
+
+//Create a int tupe Mail_box object
+void put_msg_int (uint64_t uint64_message) {
+    mail_t *mail = mail_box.alloc();
+    mail->type=1;
+    mail->uint64_message = uint64_message; 
+    mail_box.put(mail);
+}
+
+//Create a string tupe Mail_box object
+void put_msg_string (char* message) {
+    mail_t *mail = mail_box.alloc();
+    mail->type=2;
+    mail->string_message = message; 
+    mail_box.put(mail);
+}
+
+void put_msg_float (float message) {
+    mail_t *mail = mail_box.alloc();
+    mail->type=3;
+    mail->float_message = message; 
+    mail_box.put(mail);
+}
+
+//Serial read
+void serialISR(){
+    //testPin.write(1);
+    uint8_t newChar = pc.getc();
+    inCharQ.put((void*)newChar);
+    //testPin.write(0);
+}
+
+//The function the ticker calls 
+void motorCtrlTick(){ 
+    motorCtrlT.signal_set(0x1); 
+}
+
+void printNotes(void){
+    put_msg_string(notes);
+    for(int i = 0; i < number_of_notes; i++){
+        put_msg_float((float)durations[i]);
+        }
+    for(int i = 0; i < number_of_notes; i++){
+        put_msg_float((float)periods[i]);
+        }
+    }
+
+void processTuneString(void){
+        playTune_mutex.lock();
+        playTune = false;
+        playTune_mutex.unlock();
+        strcpy(tune_c_str, tune.c_str());   //turn tune (string) into c style string
+        number_of_notes = 0;
+        //reset the arrays
+        for(int i =0; i<10; i++){
+            notes[i] = '\0';
+            durations[i] = -1;
+        }
+            
+        char current_char;
+        char prev_char;
+        int tune_string_length = strlen(tune_c_str);
+        int a;
+        noteIndex  = 0;
+        
+        for(a = 0; a < tune_string_length; a++){
+            current_char = tune_c_str[a];
+            if((current_char >= 'A') &&(current_char <= 'G')){
+                notes[number_of_notes] = current_char;
+                number_of_notes++;
+            }
+            else if((current_char >= '1') && (current_char <= '9'))
+                durations[number_of_notes - 1] = (int)current_char - 48;
+                
+            else if(current_char == '#'){
+                prev_char = tune_c_str[a-1];
+                switch(prev_char){
+                    case 'A':
+                        notes[number_of_notes - 1] = 'b'; 
+                    break;
+                    case 'C':
+                        notes[number_of_notes - 1] = 'd';
+                    break;
+                    case 'D':
+                        notes[number_of_notes - 1] = 'e'; 
+                    break;  
+                    case 'F':
+                        notes[number_of_notes - 1] = 'g';  
+                    break;    
+                    case 'G':
+                        notes[number_of_notes - 1] = 'a'; 
+                    break;  
+                } 
+            }   //end of #
+            else if(current_char == '^'){
+                prev_char = tune_c_str[a-1];
+                switch(prev_char){
+                    case 'B':
+                        notes[number_of_notes - 1] = 'b';
+                    break;
+                    case 'D':
+                        notes[number_of_notes - 1] = 'd';
+                    break;
+                    case 'E':
+                        notes[number_of_notes - 1] = 'e';
+                    break;
+                    case 'G':
+                        notes[number_of_notes - 1] = 'g';
+                    break;  
+                    case 'A':
+                        notes[number_of_notes - 1] = 'a';
+                    break;
+                }//end of switch
+            }   //end of ^
+            
+        }//end of the for loop
+        for(a = 0; a < number_of_notes; a++){
+        char current_note =  notes[a];
+        switch(current_note){
+            case 'A': periods[a] = 1.0/440.0;
+            break;
+            case 'b': periods[a] = 1.0/0466.16;
+            break;
+            case 'B': periods[a] = 1.0/493.88;
+            break;
+            case 'C': periods[a] = 1.0/523.25;
+            break;
+            case 'd': periods[a] = 1.0/554.37;
+            break;
+            case 'D': periods[a] = 1.0/587.83;
+            break;
+            case 'e': periods[a] = 1.0/622.25;
+            break;
+            case 'E': periods[a] = 1.0/659.25;
+            break;
+            case 'F': periods[a] = 1.0/698.46;
+            break;
+            case 'g': periods[a] = 1.0/739.99;
+            break;
+            case 'G': periods[a] = 1.0/783.99;
+            break;
+            case 'a': periods[a] = 1.0/830.61;
+            break;
+            default: periods[a] = 1.0/440.0;
+                break;           
+            }//end of switch
+        }//end of for
+    playTune_mutex.lock();
+    playTune = true;
+    playTune_mutex.unlock();
+    return;
+}
+
+
+//Run in the motorCtrl thread
+void motorCtrlFn(){
+    char msg[50];
+    Ticker motorCtrlTicker; 
+    motorCtrlTicker.attach_us(&motorCtrlTick,100000);   //run every 100ms
+    prevPosition=motor_position;
+    //Start velocity times
+    motorCtrlTimer.start();
+    while(1){
+         //Wait for ticker message
+            
+            motorCtrlT.signal_wait(0x1);    
+            testPin.write(1);
+            motorCtrlTimer.stop();
+            timeSinceLastRun = motorCtrlTimer.read();   
+            motorCtrlTimer.reset();
+            motorCtrlTimer.start();
+            //play tune
+            
+            playTune_mutex.lock();
+            playTuneLocal = playTune;
+            playTune_mutex.unlock();
+            
+            if(playTuneLocal == true){
+                elapsedTime = elapsedTime + timeSinceLastRun;   //increment the 
+                float currentDuration = durations[noteIndex];
+                if(elapsedTime >= currentDuration){
+                    elapsedTime = 0;
+                    noteIndex++;
+                    if(noteIndex == number_of_notes)
+                        noteIndex = 0;  //reset if needed
+                    PWM_pin.period(periods[noteIndex]); //set the 
+                }  
+            }
+            else    //if playTune == false
+                PWM_pin.period(0.002f);  
+                
+                
+            //Calculate velocity
+            core_util_critical_section_enter();
+            current_position=motor_position;   
+            core_util_critical_section_exit();
+                         
+            velocity = (float)((current_position - prevPosition)/(6.0*timeSinceLastRun));    //rps
+            prevPosition=current_position;
+            
+            if((max_vel == 0) && (target_position == 0)){    //no limits            
+                lead = 2;
+                PWM_pin.write(1.0); 
+            }
+                
+            //speed limit no rotation limit, run forever at defined speed 
+            else if ((max_vel != 0) && (target_position == 0)){
+                prevVelocityError = velocityError;      
+                
+                velocityError = max_vel - abs(velocity);    //Proportional
+                differentialVelocityError = (velocityError - prevVelocityError)/timeSinceLastRun;   //Differential
+                integralVelocityError +=  velocityError*timeSinceLastRun;        // Integral
+               
+                if(integralVelocityError > VELOCITY_INTEGRAL_LIMIT)  //Limit the integral
+                    integralVelocityError = VELOCITY_INTEGRAL_LIMIT;
+                if(integralVelocityError < -1*VELOCITY_INTEGRAL_LIMIT)
+                    integralVelocityError =(-1*VELOCITY_INTEGRAL_LIMIT);
+                
+                Ts = K_PS*velocityError + K_IS*integralVelocityError + K_DS*differentialVelocityError;
+
+                lead = (Ts<0) ? -2 : 2;   
+      
+                Ts = abs(Ts);
+                if (Ts>1){
+                    Ts=1.0;
+                }
+                PWM_pin.write(Ts);
+                
+            }
+            //no speed limit, but a rotation limit
+            else if((max_vel == 0) && (target_position != 0)){
+                prevRotationError = rotationError;
+                rotationError = target_position - current_position; //in ISR TICKS
+                differentialRotationError = (rotationError - prevRotationError)/timeSinceLastRun; //in ISR ticks /s
+                
+                
+                integralRotationError +=  rotationError*timeSinceLastRun;
+                if(integralRotationError > ROTATION_INTEGRAL_LIMIT) 
+                    integralRotationError = ROTATION_INTEGRAL_LIMIT;
+                if(integralRotationError < -1*ROTATION_INTEGRAL_LIMIT) 
+                    integralRotationError = -1*ROTATION_INTEGRAL_LIMIT;
+                //put_msg_float(integralRotationError);
+                Tr = (K_PR*rotationError) + (K_IR*integralRotationError) + (K_DR*differentialRotationError); // Need to divide by time
+             
+                // In case the rotations were overshot change the direction back
+              
+                lead = (Tr > 0) ?  2 : -2;  //change direction if needed
+                Tr = abs(Tr);
+                Tr = (Tr > 1) ? 1 : Tr;     //clamp at 1.0
+                
+                if(abs(rotationError) == 0){
+                    PWM_pin.write(0.0);
+                    }
+                else
+                    PWM_pin.write(Tr); 
+                }
+            //speed limit and rotation limit
+            else{    //((max_vel != 0) && (target_position != 0)){{
+                prevRotationError = rotationError;
+                rotationError = target_position - current_position; //in ISR TICKS
+                float differentialRotationError = (rotationError - prevRotationError)/timeSinceLastRun;
+                integralRotationError +=  (rotationError*timeSinceLastRun);
+                if(integralRotationError > ROTATION_INTEGRAL_LIMIT) 
+                    integralRotationError = ROTATION_INTEGRAL_LIMIT;
+                if(integralRotationError < -1*ROTATION_INTEGRAL_LIMIT) 
+                    integralRotationError = -1*ROTATION_INTEGRAL_LIMIT;
+                //put_msg_float(integralRotationError);
+                Tr = (K_PR*rotationError) + (K_IR*integralRotationError) + (K_DR*differentialRotationError); // Need to divide by time
+                
+                prevVelocityError = velocityError;
+                velocityError = max_vel - abs(velocity);
+                float differentialVelocityError = (velocityError - prevVelocityError)/timeSinceLastRun;
+                
+                // Integral error
+                integralVelocityError +=  velocityError*timeSinceLastRun;
+                if(integralVelocityError > VELOCITY_INTEGRAL_LIMIT) 
+                    integralVelocityError = VELOCITY_INTEGRAL_LIMIT;
+                if(integralVelocityError < -1*VELOCITY_INTEGRAL_LIMIT)
+                    integralVelocityError =(-1*VELOCITY_INTEGRAL_LIMIT);
+               
+                Ts = K_PS*velocityError + K_IS*integralVelocityError + K_DS*differentialVelocityError;
+                if(differentialRotationError < 0)
+                    Ts = -Ts;
+                                   
+                    
+                if(abs(velocity) > max_vel){   
+                    newTorque = min(abs(Tr), abs(Ts));
+                    }
+                else                               
+                    newTorque = max(abs(Tr), abs(Ts));
+                    
+                if(abs(rotationError)<10)   //if we're close, take Tr
+                    newTorque = abs(Tr);
+                    
+                lead = (rotationError >= 0) ?  2 : -2;
+                
+                
+                
+                newTorque = abs(newTorque);
+                
+                if(newTorque > 1.0)
+                    newTorque = 1.0;
+                
+                if(abs(rotationError)  == 0)
+                    PWM_pin.write(0.0);
+                else
+                    PWM_pin.write(newTorque); 
+                
+            }
+             testPin.write(0);      
+    }//end of while 1
+ }//end of fn
+
+
+//Receiving command serially and decoding it 
+void readMsgFn(){
+    string message_string;
+    pc.attach(&serialISR);
+    char message[18];
+    int i=0; 
+    while(1){ 
+        osEvent newEvent = inCharQ.get();
+        //testPin.write(1);
+        uint8_t newChar = (uint8_t)newEvent.value.p;
+        if(i >= 17) 
+            i=0;
+        else{
+            message[i]=newChar;
+            i++;
+        }
+        if(newChar=='\r' || newChar == '\n'){
+            if(message[0]=='K'){
+                newKey_mutex.lock();
+                put_msg_string("Entering New Key: ");
+                sscanf(message,"K%x",&newKey);
+                message[i]='\0';
+                put_msg_int(newKey);
+                newKey_mutex.unlock();
+                i=0;
+            }
+            else if(message[0]=='R'){
+                newRotation_mutex.lock();
+                integralRotationError = 0;  //reset the sum!
+                integralVelocityError = 0;  //reset the sum!
+                sscanf(message,"R%f",&rot_input);   //rot_input in number of revs
+                if(rot_input == 0)
+                    target_position = 0;
+                else
+                    {
+                    target_position = current_position + (rot_input*6); //in ISR TICKS
+                    }
+                message[i]='\0';
+                newRotation_mutex.unlock();
+                i=0;
+            }
+            else if(message[0]=='V'){
+                newVelocity_mutex.lock();
+                sscanf(message,"V%f",&max_vel);
+                integralVelocityError = 0;  //reset the sum!
+                integralRotationError = 0;  //reset the sum!
+                newVelocity_mutex.unlock();
+                //printf("%f",max_vel);
+                message[i]='\0';
+                i=0;
+            }
+            else if(message[0] == 'T'){
+                sscanf(message,"T%s",tune);
+                if(message[1] == '0'){
+                    playTune_mutex.lock();
+                    playTune = false;
+                    playTune_mutex.unlock();
+                }
+                else
+                    processTuneString();
+                message[i] = '\0';
+                i=0;
+            }               
+        }
+         //testPin.write(0);
+    }
+} 
+
+ 
 //Main
 int main() {
-    int8_t orState = 0;    //Rotot offset at motor state 0
-    int8_t intState = 0;
-    int8_t intStateOld = 0;
+    string msg="Hello World";
+    SHA256 mySHA256 = SHA256();
+    //put_msg("HELLO");
+    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);
+    uint64_t* nonce = (uint64_t*)((int)sequence + 56);
     
-    //Initialise the serial port
-    Serial pc(SERIAL_TX, SERIAL_RX);
+    uint8_t hash[32];
+    
+    orState = 0;    //Rotot offset at motor state 0
+    intState = 0;
+    intStateOld = 0;
+    
+    //Initialise PWM and Duty Cycle;
+    PWM_pin.period(0.002f);
+    PWM_pin.write(1.00f);
+    
     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
+    I1.rise(&Rotorcalib);
+    I2.rise(&Rotorcalib);
+    I3.rise(&Rotorcalib);
+    I1.fall(&Rotorcalib);
+    I2.fall(&Rotorcalib);
+    I3.fall(&Rotorcalib);
+    
+    hashRate = 0;
+    t.start();
+    
+    printMsgT.start(callback(printMsgFn));  //start print msg thread
+    readMsgT.start(callback(readMsgFn)); 
+    motorCtrlT.start(callback(motorCtrlFn)); // start the motorCtrlT thread
+
     
-    //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
-            //pc.printf("%d\n\r",intState);
+    while (1) { 
+        //testPin.write(!testPin.read());
+        newKey_mutex.lock();
+        *key = newKey;
+        newKey_mutex.unlock();
+        
+        mySHA256.computeHash(hash,sequence,sizeof(sequence));
+        hashRate++;
+        if(t.read() >= 1){  //if a second has passed
+            t.stop();
+            t.reset();
+            t.start();
+            char msg[10];
+            strcpy(msg, "VELOCITY: ");
+            put_msg_string(msg);
+            put_msg_float(velocity);
+            char msg2[10];
+            strcpy(msg2, "HASH RATE: ");
+            put_msg_string(msg2);
+            put_msg_int(hashRate);    //hashes per second
+            hashRate = 0;
         }
+        if((hash[0] == 0) && (hash[1] == 0)){
+            char msg1[10];
+            strcpy(msg1, "SUCCESS: ");
+            put_msg_string(msg1);
+            put_msg_int(*nonce); 
+        }
+        *nonce+=1;
+        
     }
 }