
SMARTASSES 2019
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; + } }