File content as of revision 35:132413ec3d65:

#include "SHA256.h"
#include "mbed.h"
// #include <iostream>
// #include "rtos.h"

/*TODO:
Change
Indx
newCmd
MAXCMDLENGTH
move the global variables to a class because we arent paeasents - Mission Failed
*/

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

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

//Status LED
DigitalOut led1(LED1);

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

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

PwmOut pwmCtrl(PWMpin);

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

class Comm{

    public:
    
        Thread t_comm_out;
        // Thread *p_motor_ctrl;
    
        bool _RUN;
    
        RawSerial pc;
        // Queue<void, 8> inCharQ;  // Input Character Queue
    
    
        static const char MsgChar[11];
    
        uint8_t MAXCMDLENGTH;
    
        volatile uint8_t cmdIndx;
        volatile uint8_t inCharQIdx;
    
        volatile uint32_t motorPower; // motor toque
        volatile float targetVel;
        volatile float targetRot;
    
        enum msgType {motorState, posIn, velIn, posOut, velOut,
    
            hashRate, keyAdded, nonceMatch,
    
            torque, rotations,
    
            error};
    
        typedef struct {
            msgType type;
            uint32_t message;
        } msg;
    
        Mail<msg, 32> mailStack;
    
        void serialISR(){
            if (pc.readable()) {
                char newChar = pc.getc();
                // inCharQ.put((void*)newChar); // void* = pointer to an unknown type that cannot be dereferenced
    
                if (inCharQIdx == (MAXCMDLENGTH)) {
                    inCharQ[MAXCMDLENGTH] = '\0'; // force the string to have an end character
                    putMessage(error, 1);
                    inCharQIdx = 0; // reset buffer index
                    // pc.putc('\r'); // carriage return moves to the start of the line
                    // for (int i = 0; i < MAXCMDLENGTH; ++i)
                    // {
                    //     inCharQ[i] = ' ';
                    //     pc.putc(' ');
                    // }
    
                    // pc.putc('\r'); // carriage return moves to the start of the line
                }
                else{
                    if(newChar != '\r'){                //While the command is not over,
                        inCharQ[inCharQIdx] = newChar;      //save input character and
                        inCharQIdx++;                      //advance index
                        pc.putc(newChar);
                    }
                    else{
                        inCharQ[inCharQIdx] = '\0';         //When the command is finally over,
                        strncpy(newCmd, inCharQ, MAXCMDLENGTH); // Will copy 18 characters from inCharQ to newCmd
                        cmdParser();                    //parse the command for decoding.
                        for (int i = 0; i < MAXCMDLENGTH; ++i) // reset buffer
                            inCharQ[i] = ' ';
                        inCharQIdx = 0; // reset index
                    }
                }
            }
    
    
        }
    
        void returnCursor() {
            pc.putc('>');
            for (int i = 0; i < inCharQIdx; ++i) // reset cursor position
                pc.putc(inCharQ[i]);
            // for (int i = inCharQIdx; i < MAXCMDLENGTH; ++i) // fill remaining with blanks
            //         pc.putc(' ');
            // pc.putc('<');
        }
    
        void cmdParser(){
            switch(newCmd[0]) {
                case 'K': //(MsgChar[keyAdded])://
                    newKey_mutex.lock();                        //Ensure there is no deadlock
                    sscanf(newCmd, "K%x", &newKey);             //Find desired the Key code
                    putMessage(keyAdded, newKey);           //Print it out
                    newKey_mutex.unlock();
                    break;
                case 'V': //(MsgChar[velIn])://
                    sscanf(newCmd, "V%f", &targetVel);          //Find desired the target velocity
                    putMessage(velIn, targetVel);      //Print it out
                    break;
                case 'R': //(MsgChar[posIn])://
                    sscanf(newCmd, "R%f", &targetRot);          //Find desired target rotation
                    putMessage(posIn, targetRot);      //Print it out
                    break;
                case 'T': //(MsgChar[torque])://
                    sscanf(newCmd, "T%d", &motorPower);         //Find desired target torque
                    putMessage(torque, motorPower);         //Print it out
                    break;
                default: break;
            }
        }
    
        //~~~~~Decode messages to print on serial port~~~~~
        void commOutFn() {
            while (_RUN) {
                osEvent newEvent = mailStack.get();
                msg *pMessage = (msg *) newEvent.value.p;
    
                //Case switch to choose serial output based on incoming message
                switch (pMessage->type) {
                    case motorState:
                        pc.printf("\r>%s< The motor is currently in state %x\n\r", inCharQ, pMessage->message);
                        break;
                    case hashRate:
                        pc.printf("\r>%s< Mining: %.4u Hash/s\r", inCharQ, (uint32_t) pMessage->message);
                        returnCursor();
                        break;
                    case nonceMatch:
                        pc.printf("\r>%s< Nonce found: %x\r", inCharQ, pMessage->message);
                        returnCursor();
                        break;
                    case keyAdded:
                        pc.printf("\r>%s< New Key Added:\t0x%016x\n\r", inCharQ, pMessage->message);
                        break;
                    case torque:
                        pc.printf("\r>%s< Motor Torque set to:\t%d\n\r", inCharQ, pMessage->message);
                        break;
                    case velIn:
                        pc.printf("\r>%s< Target Velocity set to:\t%.2f\n\r", inCharQ, targetVel);
                        break;
                    case velOut:
                        pc.printf("\r>%s< Current Velocity:\t%.2f\n\r", inCharQ, \
                                (float) ((int32_t) pMessage->message / 6));
                        break;
                    case posIn:
                        pc.printf("\r>%s< Target Rotation set to:\t%.2f\n\r", inCharQ, \
                                (float) ((int32_t) pMessage->message / 6));
                        break;
                    case posOut:
                        pc.printf("\r>%s< Current Position:\t%.2f\n\r", inCharQ, \
                                (float) ((int32_t) pMessage->message / 6));
                        break;
                    case error:
                        pc.printf("\r>%s< Debugging position:%x\n\r", inCharQ, pMessage->message);
                        for (int i = 0; i < MAXCMDLENGTH; ++i) // reset buffer
                            inCharQ[i] = ' ';
                        break;
                    default:
                        pc.printf("\r>%s< Unknown Error. Message: %x\n\r", inCharQ, pMessage->message);
                        break;
                }
                mailStack.free(pMessage);
            }
        }
    
    
    
    
        //TODO: stop function, maybe use parent de-constructor
        //void stop_comm{}
    
        // public:
    
        volatile uint64_t newKey;   // hash key
        Mutex newKey_mutex;         // Restrict access to prevent deadlock.
    
        Comm() :  pc(SERIAL_TX, SERIAL_RX),
                  t_comm_out(osPriorityAboveNormal, 1024)
        { // inherit from the RawSerial constructor
    
            pc.printf("%s\n\r", "Welcome" );
            MAXCMDLENGTH = 18;
    
            // reset buffer
            // MbedOS prints 'Embedded Systems are fun and do awesome things!'
            // if you print a null terminator
            pc.putc('>');
            for (int i = 0; i < MAXCMDLENGTH; ++i) {
                inCharQ[i] = '.';
                pc.putc('.');
            }
            pc.putc('<');
            pc.putc('\r');
    
            inCharQ[MAXCMDLENGTH] = '\0';
            strncpy(newCmd, inCharQ, MAXCMDLENGTH);
    
            cmdIndx = 0;
    
            inCharQIdx = 0;
            // inCharQIdx = MAXCMDLENGTH-1;
    
            pc.attach(callback(this, &Comm::serialISR));
    
            // Thread t_comm_in(osPriorityAboveNormal, 1024);
            // Thread t_comm_out(osPriorityAboveNormal, 1024);
            // Thread t_motor_ctrl(osPriorityAboveNormal, 1024);
    
            motorPower = 300;
            targetVel = 45.0;
            targetRot = 459.0;
    
    
    
            /*MsgChar = {'m', 'R', 'V', 'r', 'v',
    
                      'h', 'K', 'n',
    
                      'T', 'r',
    
                      'e'};*/
        }
    
    
        void putMessage(msgType type, uint32_t message){
            msg *p_msg = mailStack.alloc();
            p_msg->type = type;
            p_msg->message = message;
            mailStack.put(p_msg);
        }
    
        void start_comm(){
            _RUN = true;
    
    
            // reset buffer
            // MbedOS prints 'Embedded Systems are fun and do awesome things!'
            // if you print a null terminator
            pc.putc('>');
            for (int i = 0; i < MAXCMDLENGTH; ++i) {
                inCharQ[i] = '.';
                pc.putc('.');
            }
            pc.putc('<');
            pc.putc('\r');
    
            inCharQ[MAXCMDLENGTH] = '\0';
            strncpy(newCmd, inCharQ, MAXCMDLENGTH);
    
            // returnCursor();
    
            // t_comm_in.start(callback(this, &Comm::commInFn));
            // this::thread::wait()
            // wait(1.0);
            t_comm_out.start(callback(this, &Comm::commOutFn));
            
    
    
        }
    
        char newCmd[];  // because unallocated must be defined at the bottom of the class
        char inCharQ[];
};


class Motor {


    protected:
        int8_t orState;            //Rotor offset at motor state 0, motor specific
        volatile int8_t currentState;    //Current Rotor State
        volatile int8_t stateList[6];    //All possible rotor states stored
    
        //Phase lead to make motor spin
        int8_t lead;
    
        Comm* p_comm;
        bool _RUN;
    
        //Run the motor synchronisation
        
        float dutyC;     // 1 = 100%
        float mtrPeriod; // motor period
        uint8_t stateCount[3];  // State Counter
        uint8_t theStates[3];   // The Key states
    
        Thread t_motor_ctrl;    // Thread for motor Control
        
        uint32_t MAXPWM;
    
    public:
    
        Motor() : t_motor_ctrl(osPriorityAboveNormal, 1024)
        {
            // Set Power to maximum to drive motorHome()
            dutyC = 1;
            mtrPeriod = 2e-3; // motor period
            pwmCtrl.period(mtrPeriod);
            pwmCtrl.pulsewidth(mtrPeriod*dutyC);
    
            orState = motorHome();             //Rotot offset at motor state 0
            currentState = readRotorState();   //Current Rotor State
            // stateList[6] = {0,0,0, 0,0,0};     //All possible rotor states stored
            lead = 2;  //2 for forwards, -2 for backwards
    
            theStates[0] = orState;
            theStates[1] = (orState + lead) % 6;
            theStates[2] = (orState + (lead*2)) % 6;
        
            stateCount[0] = 0; stateCount[1] = 0; stateCount[2] = 0;
            
            p_comm = NULL; // null pointer for now
            _RUN = false;
            
            MAXPWM = mtrPeriod*dutyC;
    
        }
    
    
        void motorStart(Comm *comm) {
    
            // Establish Photointerrupter Service Routines (auto choose next state)
            I1.fall(callback(this, &Motor::stateUpdate));
            I2.fall(callback(this, &Motor::stateUpdate));
            I3.fall(callback(this, &Motor::stateUpdate));
            I1.rise(callback(this, &Motor::stateUpdate));
            I2.rise(callback(this, &Motor::stateUpdate));
            I3.rise(callback(this, &Motor::stateUpdate));
    
            // push digitally so if motor is static it will start moving
            motorOut((currentState-orState+lead+6)%6); // We push it digitally
    
            // Default a lower duty cylce
            dutyC = 0.8;
            pwmCtrl.period(mtrPeriod);
            pwmCtrl.pulsewidth(mtrPeriod*dutyC);
    
             p_comm = comm;
            _RUN = true;

            // Start motor control thread
            t_motor_ctrl.start(callback(this, &Motor::motorCtrlFn));

            MAXPWM = mtrPeriod*dutyC;
    

        }
    
            //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];
        }
    
        //Basic synchronisation routine
        int8_t motorHome() {
            //Put the motor in drive state 0 and wait for it to stabilise
            motorOut(0);
            wait(2.0);
    
            //Get the rotor state
            return readRotorState();
        }
    
    
        void stateUpdate() { // () { // **params
            currentState = readRotorState();
    
            // Store into state counter
            if (currentState == theStates[0])
                stateCount[0]++; 
            else if (currentState == theStates[1])  
                stateCount[1]++; 
            else if (currentState == theStates[2]) 
                stateCount[2]++; 
    
    
            // (Current - Offset + lead + 6) %6
            motorOut((currentState - orState + lead + 6) % 6);
    
        }
    
    
    
        // attach_us -> runs funtion every 100ms 
        void motorCtrlFn() {
            Ticker motorCtrlTicker;
            motorCtrlTicker.attach_us(callback(this,&Motor::motorCtrlTick), 1e5);

            // Init some things
            uint8_t cpyStateCount[3]; 
            uint8_t cpyCurrentState; 

            int16_t ting[2] = {5,1}; // 360,60 (for degrees), 5,1 (for states)
            uint8_t iterElementMax;
            int16_t totalDegrees;
            int16_t stateDiff;

            int32_t velocity;                                   //Variable for local velocity calculation
            int32_t locMotorPos;                                //Local copy of motor position
            static int32_t oldMotorPos = 0;                     //Old motor position used for calculations
            static uint8_t motorCtrlCounter = 0;                //Counter to be reset every 10 iterations to get velocity calculation in seconds
            int32_t torque;                                     //Local variable to set motor torque
            float sError;                                       //Velocity error between target and reality
            float rError;                                       //Rotation error between target and reality
            static float rErrorOld;                             //Old rotation error used for calculation

            //~~~Controller constants~~~~
            int32_t Kp1=22;                                     //Proportional controller constants 
            int32_t Kp2=22;                                     //Calculated by trial and error to give optimal accuracy  
            float   Kd=15.5;    
            

            while (_RUN) {
                t_motor_ctrl.signal_wait((int32_t)0x1);
                core_util_critical_section_enter();
                //Access shared variables here
                std::copy(stateCount, stateCount+3, cpyStateCount);  
                // TODO: A thing yes
                cpyCurrentState = currentState;
                for (int i = 0; i < 3; ++i) {
                    stateCount[i] = 0; 
                }
                core_util_critical_section_exit();

                iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount; 

               
                totalDegrees = ting[0] * cpyStateCount[iterElementMax];
                stateDiff = theStates[iterElementMax]-cpyCurrentState;
                if (stateDiff >= 0) {
                    totalDegrees = totalDegrees + (ting[1]* stateDiff);  
                } else {
                    totalDegrees = totalDegrees + (ting[1]*stateDiff*-1); 
                }
                //p_comm->pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10));
            
                //~~~~~Speed controller~~~~~~
                velocity = totalDegrees*10;
                sError = (p_comm->targetVel * 6) - abs(velocity);        //Read global variable targetVel updated by interrupt and calculate error between target and reality
                int32_t Ys;                                      //Initialise controller output Ys  (s=speed)
                if (sError == -abs(velocity)) {                  //Check if user entered V0, 
                    Ys = MAXPWM;                                 //and set the output to maximum as specified
                } else {
                    Ys = (int)(Kp1 * sError);                    //If the user didn't enter V0 implement controller transfer function: Ys = Kp * (s -|v|) where,
                }                                                //Ys = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity
                
                //~~~~~Rotation control~~~~~~
                rError = p_comm->targetRot - cpyCurrentState;            //Read global variable targetRot updated by interrupt and calculate the rotation error. 
                int32_t Yr;                                      //Initialise controller output Yr (r=rotations)
                Yr = Kp2*rError + Kd*(rError - rErrorOld);       //Implement controller transfer function Ys= Kp*Er + Kd* (dEr/dt)        
                rErrorOld = rError;                              //Update rotation error
                if(rError < 0){                                  //Use the sign of the error to set controller wrt direction of rotation
                    Ys = -Ys;                               
                }

                if((velocity>=0 && Ys<Yr) || (velocity<0 && Ys>Yr)){        //Choose Ys or Yr based on distance from target value so that it takes 
                    torque = Ys;                                            //appropriate steps in the right direction to reach target value
                } else {
                    torque = Yr;
                }
                if(torque < 0){                                             //Variable torque cannot be negative since it sets the PWM  
                    torque = -torque;                                       //Hence we make the value positive, 
                    lead = -2;                                              //and instead set the direction to the opposite one
                } else {
                    lead = 2;
                }
                if(torque > MAXPWM){                                        //In case the calculated PWM is higher than our maximum 50% allowance,
                    torque = MAXPWM;                                        //Set it to our max.
                }   

                pwmCtrl.pulsewidth(torque);
                p_comm->motorPower = torque;                                        //Lastly, update global variable motorPower which is updated by interrupt        

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


int main() {

    // Declare Objects
    Comm comm_port;
    SHA256 miner;
    Motor motor;
    
    // Start Motor and Comm Port
    motor.motorStart(&comm_port);
    comm_port.start_comm();

    // Declare Hash Variables
    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];
    uint32_t length64 = 64;
    uint32_t hashCounter = 0;
    
    // Begin Main Timer
    Timer timer;
    timer.start();
    
    // Loop Program
    while (1) {
        
        // Mutex For Access Control
        comm_port.newKey_mutex.lock();
        *key = comm_port.newKey;
        comm_port.newKey_mutex.unlock();
        
        // Compute Hash and Counter
        miner.computeHash(hash, sequence, length64);
        hashCounter++;
        
        // Enum Casting and Condition
        if ((hash[0]==0) && (hash[1]==0)){
            comm_port.putMessage((Comm::msgType)7, *nonce);
        }

        // Try Nonce
        (*nonce)++;
        
        // Display via Comm Port
        if (timer.read() >= 1){
            comm_port.putMessage((Comm::msgType)5, hashCounter);
            hashCounter=0;
            timer.reset();
        }
    }
    
    return 0;
    
}