File content as of revision 5:e4b799086bc1:

#include "mbed.h"
#include "SHA256.h"
#include "rtos.h"
#include "slre.h"
#define char_len_max 32

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

//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
int8_t lead = 2;  //2 for forwards, -2 for backwards
int8_t oldLead = lead;
//Status LED

DigitalOut led1(LED1);

//Photointerrupter inputs
InterruptIn I1(I1pin);
InterruptIn I2(I2pin);
InterruptIn I3(I3pin);

//Motor Drive outputs
PwmOut L1L(L1Lpin);
DigitalOut L1H(L1Hpin);
PwmOut L2L(L2Lpin);
DigitalOut L2H(L2Hpin);
PwmOut L3L(L3Lpin);
DigitalOut L3H(L3Hpin);

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

//***********Initialisation Our Variables************//

//Message IDs
enum message_code {
    ERROR_C = 0, //Error message ID
    HASH = 1, //Hash frequency ID
    NONCE = 2, //correct nonce ID
    POSITION = 3, //Starting Rotor ID
    DECODED = 4, //Decoded message ID
    NEW_KEY = 5,
    OLD_KEY = 6,
    VELOCITY = 7,
    NEW_SPEED = 8,
    OLD_SPEED = 9,
    NEW_TORQUE = 10,
    NEW_ROTATIONS = 11
};

//message structure
typedef struct {
    uint8_t code; //ID
    uint64_t data; //Data
    float dataf; //Fudged it
} message_t;



Mail<message_t,16> outMessages; //Output message queue
Queue<void, 8> inCharQ; //character inputs

int8_t orState; //starting state of the rotor
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); //Key generation
uint64_t* nonce = (uint64_t*)((int)sequence + 56); //Nonce
uint8_t hash[32]; //Hash output
char commInChar[char_len_max]; //array 32 characters length
uint8_t ptr; //char array pointer
volatile uint64_t newKey; //means value can change between thread calls
uint64_t oldKey;
Mutex newKey_mutex; //Stops the value from beng changed during use
float newSpeed = 30.0f;
Mutex newSpeed_mutex;
uint32_t period = 2000;
uint32_t torqueVal = 1000;
int32_t kp = 25;
int32_t kp2 = 25;
int32_t kd = 20; //Why aren't we using a define??
float noRotations = 0.0f;
bool dirSwitch = false;
bool rotate = false;
bool rotStart = false;

Thread commOutT(osPriorityNormal,1024); //Output Thread
Thread commInT(osPriorityNormal,1200); //Input Thread
Thread motorCtrlT(osPriorityNormal,1024);

void init_pwm()
{
    L1L.period_us(period);
    L2L.period_us(period);
    L3L.period_us(period);
}


void putMessage(uint8_t code, uint64_t data)
{
    message_t *pMessage = outMessages.alloc(); //allocated the recieved message to  outmessages
    pMessage->code = code;
    pMessage->data = data;
    outMessages.put(pMessage);
}

void putMessage(uint8_t code, float data)
{
    message_t *pMessage = outMessages.alloc(); //allocated the recieved message to  outmessages
    pMessage->code = code;
    pMessage->dataf = data;
    outMessages.put(pMessage);
}

void commOutFn()
{
    while(1) {
        osEvent newEvent = outMessages.get(); //pulls the message
        message_t *pMessage = (message_t*)newEvent.value.p; //assigns the values to pmessage

        switch(pMessage->code) { //finds correct ID for message
            case ERROR_C:
                if(pMessage->data == 0) { //Input message was too large
                    pc.printf("Input command too large\n\r");
                } else if(pMessage->data == 1) { //Input message was too large
                    pc.printf("Key of wrong format\n\r");
                }
                break;
            case HASH:
                pc.printf("Hash Rate %d Hashes/sec \n\r",pMessage->data); //outputs the hash frequency
                break;
            case NONCE:
                pc.printf("Found a nonce 0x%016x\n\r", pMessage->data); //outputs correct nonce
                break;
            case POSITION:
                pc.printf("Rotor Starting Position: %d\n\r", pMessage->data); //outputs starting position
                break;
            case DECODED:
                if (pMessage->data == 0) {
                    pc.printf("Decoded as max speed\n\r");
                } else if (pMessage->data == 1) {
                    pc.printf("Decoded no rotations\n\r");
                } else if (pMessage->data == 2) {
                    pc.printf("Decoded key K\n\r");
                } else if (pMessage->data == 3) {
                    pc.printf("Decoded torque T\n\r");
                }
                break;
            case NEW_KEY:
                pc.printf("Decoded new key 0x%016llx\n\r",pMessage->data);
                break;
            case OLD_KEY:
                pc.printf("Decoded new key same as old key: 0x%016llx\n\r",pMessage->data);
                break;
            case VELOCITY:
                pc.printf("Current speed: %f\n\r",pMessage->dataf);
                break;
            case NEW_SPEED:
                pc.printf("New speed: %f\n\r",pMessage->dataf);
                break;
            case OLD_SPEED:
                pc.printf("New speed same as old speed: %d\n\r",pMessage->data);
                break;
            case NEW_TORQUE:
                pc.printf("New torque: %d\n\r",pMessage->data);
                break;
            case NEW_ROTATIONS:
                pc.printf("New number of rotations: %f\n\r",pMessage->dataf);
                break;
        }
        outMessages.free(pMessage); //removes the message
    }
}

void serialISR()
{
    uint8_t newChar = pc.getc(); //gets valuee from serial port
    inCharQ.put((void*)newChar); //places into newChar
}

void decode_char(char* buffer, uint8_t index)
{

    struct slre regex;
    struct cap captures[0 + 1];
    if(buffer[index] == 'V') { //if first value is R rotate cretain number of times
        putMessage(DECODED,(uint64_t)0);
        newSpeed_mutex.lock();
        sscanf(buffer, "V%f", &newSpeed);
        if(newSpeed == 0.0f) {
            newSpeed = 120.0f;
        } else if(newSpeed < 0.0f) {
            newSpeed = fabsf(newSpeed);
        }
        putMessage(NEW_SPEED,newSpeed);
        newSpeed_mutex.unlock();
    } else if(buffer[index] == 'v') { //if first value is R rotate cretain number of times
        putMessage(DECODED,(uint64_t)0);
        newSpeed_mutex.lock();
        sscanf(buffer, "v%f", &newSpeed);
        if(newSpeed == 0.0f) {
            newSpeed = 120.0f;
        } else if(newSpeed < 0.0f) {
            newSpeed = fabsf(newSpeed);
        }
        putMessage(NEW_SPEED,newSpeed);
        newSpeed_mutex.unlock();

    } else if(buffer[index] == 'R') { //if first value is V set speed of rotation
        putMessage(DECODED,(uint64_t)1);
        sscanf(buffer, "R%f", &noRotations);
        rotate = true;
        rotStart = true;
        putMessage(NEW_ROTATIONS,noRotations);
    } else if(buffer[index] == 'r') { //if first value is V set speed of rotation
        putMessage(DECODED,(uint64_t)1);
        sscanf(buffer, "r%f", &noRotations);
        rotate = true;
        rotStart = true;
        putMessage(NEW_ROTATIONS,noRotations);
    } else if (buffer[index] == 'K') { //if char is K set key to value input
        putMessage(DECODED,(uint64_t)2);
        if(!slre_compile(&regex, "K[0-9a-fA-F]{16}")) {
            putMessage(ERROR_C,(uint64_t)1);
        } else if(slre_match(&regex, buffer, 16, captures)) {
            newKey_mutex.lock();
            sscanf(buffer, "K%llx", &newKey);
            if(oldKey != newKey) {
                putMessage(NEW_KEY,newKey);
                *key = newKey;
                oldKey = newKey;
            } else {
                putMessage(OLD_KEY,oldKey);
            }
            newKey_mutex.unlock();
        } else {
            putMessage(ERROR_C,(uint64_t)1);
        }
    } else if (buffer[index] == 'k') { //if char is K set key to value input
        putMessage(DECODED,(uint64_t)2);
//        if(!slre_compile(&regex, "k[0-9a-fA-F]{16}")){
//            putMessage(ERROR_C,1);
//        }
//        else if(slre_match(&regex, buffer, char_len_max, captures)){
        newKey_mutex.lock();
        sscanf(buffer, "k%llx", &newKey);
        if(oldKey != newKey) {
            putMessage(NEW_KEY,newKey);
            *key = newKey;
            oldKey = newKey;
        } else {
            putMessage(OLD_KEY,oldKey);
        }
        newKey_mutex.unlock();
//        }
//        else {
//            putMessage(ERROR_C,1);
//        }
    } else if (buffer[index] == 'p') { //if char is K set key to value inpu
        sscanf(buffer, "p%lld", &kp);
        putMessage(NEW_TORQUE,(uint64_t)kp);
    }
}

void commInFn()
{
    pc.printf("Enter your command:\n\r"); //Tells the person to input their message
    pc.attach(&serialISR); //looks for the serialISR to get message
    while(1) {
        if(ptr >= char_len_max) {
            putMessage(ERROR_C,(uint64_t)0); //if gone over the buffer length, cancel and restart for next input
            ptr = 0; //reset pointer
            break;
        }
        osEvent newEvent = inCharQ.get(); //get next character
        uint8_t newChar = (uint8_t)newEvent.value.p;
        if(newChar != '\r' && newChar != '\n') {
            commInChar[ptr] = newChar; //place values into buffer
            ptr++; //increment pointer
        } else {
            //   commInChar[ptr] = '\0'; //defines the end of the command
            commInChar[ptr] = ' '; //defines the end of the command
            ptr = 0; //resets the pointer
            decode_char(commInChar,ptr); //sends array to decoding function
        }
    }
}

//Set a given drive state
void motorOut(int8_t driveState, uint32_t torque)
{

    //Lookup the output byte from the drive state.
    int8_t driveOut = driveTable[driveState & 0x07];

    //Turn off first
    if (~driveOut & 0x01) L1L.pulsewidth_us(0);
    if (~driveOut & 0x02) L1H = 1;
    if (~driveOut & 0x04) L2L.pulsewidth_us(0);
    if (~driveOut & 0x08) L2H = 1;
    if (~driveOut & 0x10) L3L.pulsewidth_us(0);
    if (~driveOut & 0x20) L3H = 1;

    //Then turn on
    if (driveOut & 0x01) L1L.pulsewidth_us(torque);
    if (driveOut & 0x02) L1H = 0;
    if (driveOut & 0x04) L2L.pulsewidth_us(torque);
    if (driveOut & 0x08) L2H = 0;
    if (driveOut & 0x10) L3L.pulsewidth_us(torque);
    if (driveOut & 0x20) L3H = 0;
}

//Convert photointerrupter inputs to a rotor state
inline int8_t readRotorState()
{
    return stateMap[I1 + 2*I2 + 4*I3];
}

//Basic synchronisation routine
int8_t motorHome()
{
    //Put the motor in drive state 0 and wait for it to stabilise
    //motorOut(0,torqueVal);
    motorOut(0,1000);
    wait(1.0);
    lead = 0;

    //Get the rotor state
    return readRotorState();
}

int32_t motorPosition;
void motorISR()
{
    static int8_t oldRotorState;
    int8_t rotorState = readRotorState(); //reads motor position
    motorOut((rotorState-orState+lead+6)%6,torqueVal); //+6 to make sure the remainder is positive
    if(rotorState - oldRotorState==5) motorPosition--;
    else if(rotorState - oldRotorState== -5) motorPosition++;
    else motorPosition+=(rotorState - oldRotorState);
    oldRotorState = rotorState;
}

void motorCtrlTick()
{
    motorCtrlT.signal_set(0x1);
}

Timer t_motor;
void motorCtrlFn()
{

    float v, v_avg, ys, yr, dEr;
    int i = 0, dt, oldPosition, totPosition, position, startPosition, newEr, oldEr, mainLead;
    bool jacobFudge = false;
    t_motor.start();
    Ticker motorCtrlTicker;
    motorCtrlTicker.attach_us(&motorCtrlTick,100000);
    while(1) {
        motorCtrlT.signal_wait(0x1);
        if(rotate) {
            if(rotStart) {
                if(noRotations > 0) {
                    lead = 2;
                } else if(noRotations < 0) {
                    lead = -2;
                } else if(noRotations == 0 && lead == 0) {
                    lead = 2;
                }
                i = 0;
                v_avg = 0;
                yr = 0.0f;
                ys = 0.0f;
                dEr = 0.0f;
                mainLead = lead; //sets general direction
                totPosition = (int)6*noRotations; //nimber of position changes required
                oldEr = totPosition; //how far away
                rotStart = false; //stops from running this loop
                __disable_irq(); //disables interrupts
                startPosition = motorPosition; //sets start position at present motor position
                oldPosition = startPosition; //sets old position to same value
                t_motor.reset();
                motorPosition = 0; //resets time and motorPosition
                __enable_irq(); //enables interrupts
                position = 0;
                jacobFudge = true;

            } else if(noRotations == 0) { //if to spin forever
                i++; //increment counter
                __disable_irq();
                position = motorPosition; //adds on number of rotations
                dt = t_motor.read_ms(); //change in time
                t_motor.reset(); //resets time
                motorPosition = 0; //resets motor position
                __enable_irq(); //enables interrupts
                v = (166.67f*((float)position/(float)dt)); //calculates velocity
                v_avg += v; //adds speed onto averager
                newSpeed_mutex.lock();
                if((int)abs(v) < 1 && newSpeed != 0) {
                    lead = mainLead; //makes sure it's in the correct direction
                    torqueVal = 1000; //sets torque
                    motorISR(); //moves the motor
                }
                newSpeed_mutex.unlock();
                ys = kp*(newSpeed-abs(v)); //speed controller
                if(ys < 0) {
                    lead = mainLead*-1;
                } else {
                    lead = mainLead;
                }
                if(abs(ys) > 1000) {
                    torqueVal = 1000;
                } else {
                    torqueVal = abs(ys);
                }
                // pc.printf("torque = %d\r\n",torqueVal);
            } else {
                i++; //increment counter
                __disable_irq();
                position +=  motorPosition; //adds on number of rotations
                dt = t_motor.read_ms(); //change in time
                //       pc.printf("motorPosPre = %d\r\n",motorPosition);
                t_motor.reset(); //resets time
                motorPosition = 0; //resets motor position
                __enable_irq(); //enables interrupts
                //      pc.printf("Pos = %d\r\n",position);
                v = 166.67f*(((float)position-(float)oldPosition)/(float)dt); //calculates velocity

                oldPosition = position; //changes old position
                newEr = totPosition-position; //difference in placement
                dEr = 1000.0f*((float)newEr-(float)oldEr)/(float)dt; //change against time
                oldEr = newEr; //old is same as new
                yr = (float)kp2*(float)newEr + (float)kd*dEr; //rotational controller
                v_avg += v; //adds speed onto averager
                ys = (float)kp*((float)newSpeed-fabsf(v))*((newEr > 0) ? 1.0f : ((newEr < 0) ? -1.0f : 0.0f)); //speed controller
                if(jacobFudge == true) {
                    lead = mainLead; //makes sure it's in the correct direction
                    torqueVal = 900; //sets torque
                    motorISR(); //moves the motor
                    jacobFudge = false;

                }
                if(v >=0.0f) { //if speed is +ve
                    if(abs(ys)<abs(yr)) {
                        torqueVal = abs(ys);
                    } else {
                        torqueVal = abs(yr);
                    }
                    if(abs(newEr) <=5) {
                        torqueVal = 0;
                        lead = 0;
                        rotate = false;
                        pc.printf("NewErr %d\r\n",newEr);
                    } else if (yr<-500 && dEr <0)
                        torqueVal = abs(yr);
                        lead = -2;
                    } else {
                        lead = 2;
                    }
                } else {
                    if(abs(ys)>abs(yr)) {
                        torqueVal = abs(ys);
                    } else {
                        torqueVal = abs(yr);
                    }
                    if(abs(newEr) <=5) {
                        torqueVal = 0;
                        lead = 0;
                        rotate = false;
                        pc.printf("NewErr %d\r\n",newEr);
                    } else if (yr>500 && dEr >0)
                        torqueVal = abs(yr);
                        lead = 2;
                    } else {
                        lead = -2;
                    }
                }
                if(torqueVal != 0 && lead !=0 && abs(v)== 0) {
                    torqueVal = torqueVal + 50;
                    motorISR();
                }
                if(torqueVal > 1000) {
                    torqueVal = 1000;
                }
            }
        }
        if (i==10) {
            v_avg = v_avg/i;
            putMessage(VELOCITY, v_avg);
            v_avg = 0;
            i= 0;
        }
    }
}


//Main
int main()
{
    pc.printf("Hello\n\r"); //outputs hello when turned on
    init_pwm();
    commOutT.start(commOutFn); //starts the output and input threads
    commInT.start(commInFn);
    //Run the motor synchronisation
    orState = motorHome(); //finds staring position
    putMessage(POSITION,(uint64_t)orState);

    Timer t; //adds a timer to count number of hashes per second
    //orState is subtracted from future rotor state inputs to align rotor and motor states
    //Poll the rotor state and set the motor outputs accordingly to spin the motor
    I1.rise(&motorISR); //looks for rising edge to trigger the motor change
    I2.rise(&motorISR);
    I3.rise(&motorISR);
    I1.fall(&motorISR); //looks for rising edge to trigger the motor change
    I2.fall(&motorISR);
    I3.fall(&motorISR);
    uint16_t counter;
    counter = 0; //initialised and set to 0 to count number of hashes
    t.start(); //starts the timer
    motorCtrlT.start(motorCtrlFn);
    while (1) {

        if(t.read_ms() >= 1000) { //if more than 1 second has surpased
            putMessage(HASH, (uint64_t)counter); //outputs the hash frequency
            counter = 0; //reset counter
            t.reset(); //resets the timer
        }
        SHA256::computeHash(&hash[0],&sequence[0],sizeof(sequence)); //computes the hash
        counter++; //increments counter;

        if((hash[0] == 0) && (hash[1] == 0)) {
            putMessage(NONCE,*nonce); //when hash is correct print the nonce
        }

        *nonce += 1; //increments nonce
    }
}