File content as of revision 22:997c013e0f13:

/*_________________________________LIBRARIES__________________________________*/

#include "mbed.h"
#include "rtos.h"
#include "PID.h"

#include "ctype.h"
#include <string>
#include "stdlib.h"
#include "math.h"
#include <limits>
//#include <vector>

/*_________________________________PIN SETUP__________________________________*/

//PhotoInterrupter Input Pins
#define I1pin D2
#define I2pin D11
#define I3pin D12

//Incremental Encoder Input Pins
#define CHA   D7
#define CHB   D8  

//Motor Drive output pins   //Mask in output byte
#define L1Lpin D4           //0x01
#define L1Hpin D5           //0x02
#define L2Lpin D3           //0x04
#define L2Hpin D6           //0x08
#define L3Lpin D9           //0x10
#define L3Hpin D10          //0x20


//Photointerrupter Inputs as Interrupts 
InterruptIn InterruptI1(D2);
InterruptIn InterruptI2(D11);
InterruptIn InterruptI3(D12);

//Incremental Encoder Inputs as Interrupts 
InterruptIn InterruptCHA(D7);
DigitalIn InterruptCHB(D8);

//Motor Drive Outputs in PWM
PwmOut L1L(L1Lpin);
PwmOut L1H(L1Hpin);
PwmOut L2L(L2Lpin);
PwmOut L2H(L2Hpin);
PwmOut L3L(L3Lpin);
PwmOut L3H(L3Hpin);

PwmOut testOut(A5);

//Status LED
//DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);

DigitalOut TIME(D13);   //Toggle Digital Pin to measure Interrupt Times 

//Initialise the serial port
Serial pc(SERIAL_TX, SERIAL_RX);

//Timer 
Timer rps;          // Measures Time for complete revolution 
Timer partial_rps;  // Measures Time for partial revolutions 
Timer tmp;          // Profiler Timer 

//PID Controller 
PID velocity_pid(0.75, 0.025, 0.2, 0.1);  // (P, I, D, WAIT)
PID dist_pid(2, 0.0, 0.01, 0.1);        // (P, I, D, WAIT)

//Initialize Threads 
//Thread pid_thread(osPriorityNormal, 512, NULL);
Thread melody_thread(osPriorityNormal, 512, NULL);

typedef struct{
    char note;
    uint8_t sharp;
    uint8_t oct;
    float dur;
}note_t;



/*________________________Motor Drive States__________________________________*/

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

/*____________________Global Variable Initialization__________________________*/

//Rotor Direction Default 
const int8_t lead = -2;         //Phase lead to make motor spin: 2 for forwards, -2 for backwards   
int8_t direction = 1;           //+1: Backwards rotation; -1 for Forwards Rotation

//Optical Disk States 
uint8_t orState=0;              //Offset of Motor Field and Optical Disk
uint8_t intState=0;             //Current Optical Disk state 
const uint8_t num_states = 6;   //Number of states in one rotation

uint32_t count = 0;             //Counts number of states traversed 
int8_t completed = 0;           //Checks if rotation completed 
int8_t driveto = 0;             //Holds value of new motor drive state 

//Angular Velocity Variables 
float PWM_freq = 0.00001f;        //100kHz (> Motor LP cut-off frequency = 10Hz) 

float dutyout = 1.0f;           //Initialized at 50% duty cycle
float dutyout_max = 1.0f;       //Maximum Duty Cycle will enable maximum speed

float angular_vel = 0.0f;       //Revolution per second (Measured over 360)
float partial_vel = 0.0f;       //Revolution per second (Measured over 360/117)

float vel_target = 0.0f;        //Target Speed  
float vel_max = 100;            //Maximum Speed at 3.0V achievable is ~60 rps

//Position Variables 
uint32_t revstates_count = 0;   //Global Variable to pass into interrupt
uint8_t pulse_count = 0;        //Max.Pulse count = 117  

float total_rev = 0.0f;         
float partial_rev = 0.0f;

float rev_target = 0.0f;        //Target Rotations 
float revstates_max = 0;

//Debug Variables 
bool flag = false;
float test_time = 0.0f;
int8_t test = 0;
int songLen = 0;

// Timer interrupt
Ticker calcPID_timer;

/*_____Basic Functions (Motor Drive, Synchronization, Reading Rotor State)____*/

//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 (PWM)
    if (~driveOut & 0x01) L1L = 0; 
    if (~driveOut & 0x02) L1H.write(dutyout); L1H.period(PWM_freq); 
    if (~driveOut & 0x04) L2L = 0;
    if (~driveOut & 0x08) L2H.write(dutyout); L2H.period(PWM_freq);
    if (~driveOut & 0x10) L3L = 0; 
    if (~driveOut & 0x20) L3H.write(dutyout); L3H.period(PWM_freq);
    
    //Then turn on (PWM)
    if (driveOut & 0x01) L1L.write(dutyout); L1L.period(PWM_freq);
    if (driveOut & 0x02) L1H = 0; 
    if (driveOut & 0x04) L2L.write(dutyout); L2L.period(PWM_freq); 
    if (driveOut & 0x08) L2H = 0; 
    if (driveOut & 0x10) L3L.write(dutyout); L3L.period(PWM_freq); 
    if (driveOut & 0x20) L3H = 0; 

    testOut.write(0.5f);
    testOut.period(PWM_freq);
}

//Convert photointerrupter inputs to a rotor state
inline int8_t readRotorState(){
    return stateMap[InterruptI1.read() + 2*InterruptI2.read() + 4*InterruptI3.read()];
}

const float octaveMap[] = {27.50, 30.87, 16.35, 18.35, 20.60, 21.83, 24.50};

float returnNote(const note_t &n)
{
    unsigned octMult = 1;
    octMult = octMult << n.oct;
    return 1.0f/( octMult*octaveMap[n.note - 'A'] );
}

void playMelody(note_t* song){
    for (int i=0; i<songLen; ++i){
        //pc.printf("Playing note %c with duration %f and period %f\r\n", song[i].note, song[i].dur, returnNote(song[i]));
        PWM_freq = returnNote(song[i]);
        wait(song[i].dur);
    }
    PWM_freq = 0.00001f;
    delete[] song;
}

void calculatePID(){
    //Calculate new PID Control Point
    if((total_rev/rev_target) > 0.75f){
        dist_pid.setProcessValue(total_rev);
        dutyout = dist_pid.compute();
    }
    else{
        velocity_pid.setProcessValue(partial_vel);
        dutyout = velocity_pid.compute();
    }   
}

//Basic synchronisation routine    
int8_t motorHome() {
    //Put the motor in drive state X (e.g. 5) to avoid initial jitter
    //Set to maximum speed to get maximum momentum 
    dutyout = 1.0f;
    motorOut(5);
    wait(1.0);
    
    //Put the motor in drive state 0 and wait for it to stabilise
    motorOut(0);
    wait(1.0);
    
    //Get the rotor state
    return readRotorState();
}

/*________________Advanced Functions (Speed and Position Control)_____________*/

// Function involves PID 
void position_control(float rev, float vel){
    
    completed = 0;
    dutyout = 1.0f;
    count = 0;
    
    rev_target = rev;
    vel_target = vel;
    //pc.printf("rev %f\r\n", rev); 
    //pc.printf("vel %f\r\n", vel); 
    
    //Reverses motor direction if forwards rotation requested 
    if (rev_target < 0.0f || vel_target < 0.0f){
        direction = -1; 
        if (rev_target < 0.0f)
            rev_target = rev_target * -1;
        else
            vel_target = vel_target * -1;
    }
    else{
        direction = 1;
    }
    
    //pc.printf("vel_target %f\r\n", vel_target); 
    //pc.printf("dir %d\r\n", direction); 
    
    pc.printf("Waiting for stabilize...\r\n"); 
    dutyout = 0.0f;
    velocity_pid.reset();
    dist_pid.reset();
    wait(3);
    motorHome();
    pc.printf("Restarting...\r\n"); 

    velocity_pid.reset();
    //velocity_pid.setInputLimits(0.0, 50);
    velocity_pid.setOutputLimits(0.0, 1.0);
    velocity_pid.setMode(1);
    velocity_pid.setSetPoint(vel_target);
    
    dist_pid.reset();
    //dist_pid.setInputLimits(0.0, rev_target);
    dist_pid.setOutputLimits(0.0, 1.0);
    dist_pid.setMode(1);
    dist_pid.setSetPoint(rev_target);

    dutyout = 0.3f;
    intState = readRotorState();
    driveto = (intState-orState+(direction*lead)+6)%6; 
    motorOut(driveto);
    
    calcPID_timer.attach(&calculatePID, 0.05);
}

void changestate_isr(){
    //TIME = 1;  
    //led2 = !led2;
    // Profiling: Test time duration of ISR 
    /*if(test == 0){
        tmp.start();
        test = 1;
    }
    
    else{
        tmp.stop();
        test_time = tmp.read();
        tmp.reset();
        test = 0;
    }*/
    
    // Measure time for 360 Rotation 
    if(driveto == 0x04){    //Next time drivestate=4, 360 degrees revolution 
        pulse_count = 0;
        /*if(flag){
            rps.stop();
            angular_vel = 1/(rps.read());
            rps.reset();
            flag = 0;
        }*/
    }
    
    /*if(driveto == 0x04){    //First time drivestate=4, Timer started at 0 degrees
        pulse_count = 0;    //Synchronize Quadrature Encoder with PhotoInterrupter 
        rps.start();
        flag = 1; 
    }*/
    
    // Measure number of revolutions 
    count++;                 
    
    if (rev_target == std::numeric_limits<float>::max()){
        total_rev = 0.0f;
    }
    
    //Turn-off when target reached (if target is 0, never end)                    
    if(completed || total_rev >= rev_target){
        completed = 1;
        total_rev = 0;
        count = 0;
        dutyout = 0;
        motorOut(0);
        led3 = 0;
        partial_rev = 0;
        calcPID_timer.detach();
    }
    else{    
        intState = readRotorState();
        driveto = (intState-orState+(direction*lead)+6)%6;
        motorOut(driveto);
    }
    //TIME = 0;  
}

void pid_isr(){
    //TIME = 1;  
    //117 Pulses per revolution
    pulse_count++;           
    
    //Measure Time to do 3 degrees of rotation
    if(test == 0){
        partial_rps.start();
        test = 1;
    }
    else{
        partial_rps.stop();
        partial_vel = 1/((117.0f * partial_rps.read()));
        partial_rps.reset();
        test = 0;
    }
    
    //Partial Revolution Count 
    partial_rev = pulse_count/117.0f;
    
    //Total Revolution Count 
    total_rev = (count/6.0f) + partial_rev; 
    //TIME = 0;
}

/*__________________________Main Function_____________________________________*/ 

void serial_com(){
    
    pc.baud(9600);
    float r=0;
    float v=0; //velocity
    bool r_val=true;
    bool v_val=true;
    int16_t t_loc=0;
    int16_t r_loc=0;
    int16_t v_loc=0;
    char buf[20];
    
    bool note_marker;
    bool dur_marker;
    bool accent_marker;
    string note="";
    uint8_t duration=0;
    Timer t;
    t.start();
    string input;
    while(1){  
        r=0;
        v=0;
        r_val=true;
        v_val=true;
        note_marker=false;
        dur_marker=false;
        accent_marker=false;
        t.stop();        
        pc.printf("Time taken was %f seconds. \r\n Please enter something\r\n", t.read());
        pc.scanf("%s",&buf);
        t.reset();
        t.start();
        input=buf;
        pc.printf("The input string is %s\r\n",buf);

        t_loc=input.find('T');
        r_loc=input.find('R');
        v_loc=input.find('V');
        //pc.printf("Location of T is %d\r\n",t_loc);
        //pc.printf("Location of R is %d\r\n",r_loc);
        //pc.printf("Location of V is %d\r\n",v_loc);

        if(t_loc==0 && input.length()>1){ //if melody marker present and there is something after it
            //printf("Note sequence detected\r\n"); 
            //note_t* song = (note_t*)malloc(sizeof(note_t)*input.length());
            note_t* song = new note_t[input.length()/2];    // since each note is at least 2 characters long, max length is string/2
            songLen = 0;
            for(int i=1;i<input.length();i++){
            
                if(accent_marker==false && dur_marker==false && note_marker==false && 
                    (input[i]=='A' || input[i]=='B' || input[i]=='C' || input[i]=='D' || input[i]=='E' || input[i]=='F' || input[i]=='G')){
                    note_marker=true;
                }
                else if(note_marker==true && (input[i]=='^' || input[i]=='#')){
                    accent_marker=true;
                }
                else if((note_marker==true && isdigit(input[i]) && accent_marker==false) || 
                    (note_marker==true && isdigit(input[i]) && accent_marker==true)){
                    dur_marker=true;    
                }
                
                if(note_marker==true && accent_marker==true && dur_marker == true){
                    note=input.substr(i-2,2);
                    //printf("The note is %s\r\n",note.c_str());
                    duration=atof(input.substr(i,1).c_str());
                    //printf("Duration is %d\r\n",duration);
                    note_marker=false;
                    dur_marker=false;
                    accent_marker=false;
                    note_t newNote = {note[0], 1, 5, duration};
                    song[songLen++] = newNote;
                }
                else if(note_marker==true && dur_marker==true && accent_marker==false){
                    note=input.substr(i-1,1);
                    //printf("The note is %s\r\n",note.c_str());
                    duration=atof(input.substr(i,1).c_str());
                    //printf("Duration is %d\r\n",duration);
                    note_marker=false;
                    dur_marker=false;
                    accent_marker=false;
                    note_t newNote = {note[0], 1, 5, duration};
                    song[songLen++] = newNote;
                }
            }
            melody_thread.start(callback(playMelody, song));
        }   
        else if(t_loc==-1){ //if no melody marker present
            //pc.printf("Note sequence NOT detected\r\n");
            
            if(r_loc==0 && v_loc==-1 && input.length()>1){ //check if first letter is R
                //pc.printf("Checking for sole R input type...\r\n");
                
                for(int j=1; j<input.length();j++){
                    if(!isdigit(input[j]) && input[j]!='-' && input[j]!='.'){
                            r_val=false;
                    }
                }
                
                if(r_val==true){
                    r=atof(input.substr(1).c_str());
                    pc.printf("Spin for %.3f number of rotations\r\n",r);
                    position_control((float) r, vel_max);
                }
                else{
                    pc.printf("Invalid input\r\n");
                }        
            }
            else if(r_loc==0 && v_loc!=-1 && v_loc < input.length()-1){ //check if first letter is R and V is also present
                //pc.printf("Checking for combined R and V input type...\r\n");
                
                for(int j=1; j<v_loc;j++){
                    if(!isdigit(input[j]) && input[j]!='-' && input[j]!='.'){
                        r_val=false;
                    }   
                }
                for(int j=v_loc+1; j<input.length();j++){
                    if(!isdigit(input[j]) && input[j]!='-' && input[j]!='.'){
                        v_val=false;
                    }
                }
    
                if(r_val==true && v_val==true){
                    r=atof(input.substr(1,v_loc-1).c_str());
                    v=atof(input.substr(v_loc+1).c_str());
                    if(v<0){
                        v=abs(v);
                    }
                    pc.printf("Spin for %.3f number of rotations at %.3f speed \r\n",r,v);
                    
                    position_control((float) r, (float) v);
                }
                else{
                    pc.printf("Invalid input\r\n");
                }
            }
            else if(v_loc==0 && input.length()>1){ //check if first letter is V
                //pc.printf("Checking for sole V input type...\r\n");
                for(int j=1; j<input.length();j++){
                    if(!isdigit(input[j]) && input[j]!='-' && input[j]!='.'){
                        v_val=false;
                    }    
                }
                if(v_val==true){
                    v=atof(input.substr(1).c_str());
                    pc.printf("Spin at %.3f speed\r\n",v);
                    position_control(std::numeric_limits<float>::max(),(float)v);
                }
                else{
                    pc.printf("Invalid input\r\n");
                }
            }
            else{
                pc.printf("Invalid input\r\n");
            }
        }
    }
}


int main(){
    
    //Start of Program
    pc.printf("STARTING SKAFMO BRUSHLESS MOTOR PROJECT! \n\r");   
    led3 = 0;
    
    //Run the motor synchronisation: orState is subtracted from future rotor state inputs
    orState = motorHome();
    //pc.printf("Synchronization Complete: Rotor and Motor aligned with Offset: %d\r\n", orState);
    
    //Interrupts (Optical Disk State Change): Drives to next state, Measures whole revolution count, Measures angular velocity over a whole revolution 
    InterruptI1.rise(&changestate_isr);
    InterruptI1.fall(&changestate_isr);
    InterruptI2.rise(&changestate_isr);
    InterruptI2.fall(&changestate_isr);
    InterruptI3.rise(&changestate_isr);
    InterruptI3.fall(&changestate_isr);
    
    //Interrupts (Incremental Encoder CHA Phase)
    InterruptCHA.rise(&pid_isr);
    
    //Initial Target Settings 
    //float rotation_set = 100.00;
    //float velocity_set = 10.00;
    
    // Melody in a Thread
    // PID in Thread
    
    //If speed not defined, use vel_max! If Rotation not defined, use revstates_max
    //float rotation_set = revstates_max;
    //float velocity_set = vel_max;
    
    
    //pid_thread.join();
    serial_com();
    
}



/*_______________________Testing and Tuning Function__________________________*/

/*Measures Angular Velocity using PhotoInterrupters by checking time taken to go 
  from State 4 to State 4 in this case. Avoid sensor phasing as it measures one 
  complete cycle */
  
void meas_velocity(){
    
    intState = readRotorState();
    driveto = (intState-orState+(direction*lead)+6)%6; 
    motorOut(driveto);
    
    while (1) {
        
        pc.printf("Rotations per second: %f \n\r", angular_vel);  
          
    }
}

// Function has no PID 
void rotation_control(int8_t num_revs, int8_t sign){
    
    revstates_count = num_revs*num_states;
    
    intState = readRotorState();
    driveto = (intState-orState+(sign*lead)+6)%6; 
    motorOut(driveto); 
          
    while(!completed){
        
        //pc.printf("Angular velocity: %f \n", angular_vel);
        pc.printf("Partial Angular: %f \n", partial_vel);        
        //pc.printf("Count: %d \r\n", (count/6));
        
    }
}

void PID_tuning(){

    dutyout = 0.5;
    
    intState = readRotorState();
    driveto = (intState-orState+lead+6)%6; 
    motorOut(driveto);
    
    while (1) {
        
        // Testing Step Response by increasing D.C. from 0.5 to 0.7 
        // Gradient is equal to Kc
        
        if(count > 3000){
            dutyout = 0.7;    
        }
        
        pc.printf("Duty Cycle: %f ", dutyout);
        pc.printf("Rotations per second: %f ", angular_vel);
        pc.printf("Count: %d \n\r", count);
        
    }
}