Omar Muttawa
SMARTASSES 2019
Diff: main.cpp
- Revision:
- 11:aae7c9c290e2
- Parent:
- 10:a4b5723b6c9d
- Child:
- 12:b39f69ed20af
--- 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;
+
}
}