File content as of revision 12:b39f69ed20af:

//SMARTASSES

#include "mbed.h"
#include "SHA256.h"
#include <string>
#include <vector>
#include <RawSerial.h>

//Photointerrupter input pins
#define I1pin D3
#define I2pin D6
#define I3pin D5

//Incremental encoder input pins
#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
#define L2Hpin A6          //0x08
#define L3Lpin D10         //0x10
#define L3Hpin D2          //0x20

#define PWMpin D9

//Motor current sense
#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
0       H   -   L
1       -   H   L
2       L   H   -
3       L   -   H
4       -   L   H
5       H   L   -
6       -   -   -
7       -   -   -
*/
//Drive state to output table
const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};

//Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};  
//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
volatile int8_t lead = 2;  //2 for forwards, -2 for backwards

//Status LED
DigitalOut led1(LED1);
DigitalOut testPin(TESTPIN);

//Photointerrupter inputs
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);
DigitalOut L1H(L1Hpin);
DigitalOut L2L(L2Lpin);
DigitalOut L2H(L2Hpin);
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){
    
    //Lookup the output byte from the drive state.
    int8_t driveOut = driveTable[driveState & 0x07];
      
    //Turn off first
    if (~driveOut & 0x01) L1L = 0;
    if (~driveOut & 0x02) L1H = 1;
    if (~driveOut & 0x04) L2L = 0;
    if (~driveOut & 0x08) L2H = 1;
    if (~driveOut & 0x10) L3L = 0;
    if (~driveOut & 0x20) L3H = 1;
    
    //Then turn on
    if (driveOut & 0x01) L1L = 1;
    if (driveOut & 0x02) L1H = 0;
    if (driveOut & 0x04) L2L = 1;
    if (driveOut & 0x08) L2H = 0;
    if (driveOut & 0x10) L3L = 1;
    if (driveOut & 0x20) L3H = 0;
    }
    
//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() {
    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);
    
    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

    
    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;
        
    }
}