File content as of revision 19:805c87360b55:

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

/*TODO:
Change 
Indx
newCmd
MAXCMDLENGTH
*/

//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       -   -   -
*/
//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
const int8_t lead = 2;  //2 for forwards, -2 for backwards

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


//Declare and start threads
class T_{
    
    protected:

        uint32_t motorPower; // motor toque
        float targetVel;
        float targetRot;

        Thread *p_comm_in;
        Thread *p_comm_out;
        Thread *p_motor_ctrl;
    
    public:
        
        T_(){
            //(priority, stack size, 
            Thread comm_in(osPriorityAboveNormal, 1024);
            Thread comm_out(osPriorityAboveNormal, 1024);
            Thread motor_ctrl(osPriorityAboveNormal, 1024);
            
            p_comm_in = &comm_in;
            p_comm_out = &comm_out;
            p_motor_ctrl = &motor_ctrl;

            motorPower = 300;
            targetVel = 45.0;
            targetRot = 459.0;
            
        }

        ~T_(){
            if (p_comm_in->get_state() == 2)
                p_comm_in->terminate();
            if (p_comm_out->get_state() == 2)
                p_comm_out->terminate();
            if (p_motor_ctrl->get_state() == 2)
                p_motor_ctrl->terminate();
        }
};


class Motor : public T_{

    private:
            int32_t MAXPWM;
            int8_t orState;              // Rotor offset at motor state 0 
            int8_t intStateOld;          // Motor old state, may change in ISR 

            int32_t motorPos;


    public:

        Motor(){
            MAXPWM = 1000;
            orState = 0;    
            intStateOld = 0;
        }


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

            //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(pw);
            if (driveOut & 0x02) L1H = 0;
            if (driveOut & 0x04) L2L.pulsewidth_us(pw);
            if (driveOut & 0x08) L2H = 0;
            if (driveOut & 0x10) L3L.pulsewidth_us(pw);
            if (driveOut & 0x20) L3H = 0;
        }

        inline int8_t readRotorState(){
            return stateMap[I1 + 2*I2 + 4*I3];
        }

        int8_t motorHome() {
            //Put the motor in drive state 0 and wait for it to stabilise
            motorOut(0, MAXPWM); // set to max PWM
            wait(2.0);
            
            //Get the rotor state
            return readRotorState();
        }

        void motorISR() {
            static int8_t oldRotorState;
            int8_t rotorState = readRotorState();
            
            motorOut((rotorState-orState+lead+6)%6,motorPower);
            
            // update motorPosition and oldRotorState
            if (rotorState - oldRotorState == 5) motorPos--;
            else if (rotorState - oldRotorState == -5) motorPos++;
            else motorPos += (rotorState - oldRotorState);
            oldRotorState = rotorState;
        }

        void stateUpdate(int8_t *params[]) { // () { // **params
            *params[0] = readRotorState(); 
            int8_t currentState = *params[0];
            int8_t offset = *params[1];
            
            motorOut((currentState - offset + lead + 6) % 6, MAXPWM);
        }
};


class Comm : public T_{
    
    private:
        bool _RUN;

        RawSerial pc;
        Queue<void, 8> inCharQ;  // Input Character Queue

        volatile uint64_t newKey;   // hash key
        Mutex newKey_mutex;         // Restrict access to prevent deadlock.

        static const char MsgChar[11];
        
        uint8_t MAXCMDLENGTH;
        char newCmd[]; 
        uint8_t cmdIndx;



        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(){ 
            uint8_t newChar = pc.getc(); 
            inCharQ.put((void*)newChar); 
        } 

        void commInFn() {
            if (_RUN)
                pc.attach(callback(this, &Comm::serialISR));
            while (_RUN) {
                osEvent newEvent = inCharQ.get();
                uint8_t newChar = ((uint8_t)(&newEvent.value.p));
                pc.putc(newChar);
                if(cmdIndx >= MAXCMDLENGTH){            //Make sure there is no overflow in comand.
                    cmdIndx = 0;
                    putMessage(error, 1);
                }
                else{
                    if(newChar != '\r'){                //While the command is not over, 
                        newCmd[cmdIndx] = newChar;      //save input character and
                        cmdIndx++;                      //advance index
                    } 
                    else{
                        newCmd[cmdIndx] = '\0';         //When the command is finally over,
                        cmdIndx = 0;                    //reset index and
                        cmdParser();                    //parse the command for decoding.
                    }
                }
            }
        }
        
        void cmdParser(){
            switch(newCmd[0]) {
                case 'K':
                        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':
                        sscanf(newCmd, "V%f", &targetVel);          //Find desired the target velocity
                        putMessage(velIn, targetVel);      //Print it out
                        break;
                case 'R':
                        sscanf(newCmd, "R%f", &targetRot);          //Find desired target rotation
                        putMessage(posIn, targetRot);      //Print it out
                        break;
                case 'T':
                        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; // ADEL ??

                //Case switch to choose serial output based on incoming message
                switch(pMessage->type) {
                    case motorState:
                        pc.printf("The motor is currently in state %x\n\r", pMessage->message);
                        break;
                    case hashRate:
                        pc.printf("Mining at a rate of %.2f Hash/s\n\r", (int32_t)pMessage->message);
                        break;
                    case nonceMatch:
                        pc.printf("Nonce found: %x\n\r", pMessage->message);
                        break;
                    case keyAdded:
                        pc.printf("New key added:\t0x%016x\n\r", pMessage->message);
                        break;
                    case torque:
                        pc.printf("Motor torque set to:\t%d\n\r", pMessage->message);
                        break;
                    case velIn:
                        pc.printf("Target velocity set to:\t%.2f\n\r", targetVel);
                        break;
                    case velOut:
                        pc.printf("Current Velocity:\t%.2f\n\r", \
                            (float)((int32_t)pMessage->message / 6));
                        break;
                    case posIn:
                        pc.printf("Target rotation set to:\t%.2f\n\r", \
                            (float)((int32_t)pMessage->message / 6));
                        break;
                    case posOut:
                        pc.printf("Current position:\t%.2f\n\r", \
                            (float)((int32_t)pMessage->message / 6));
                        break;
                    case error:
                        pc.printf("Debugging position:%x\n\r", pMessage->message);
                        break;
                    default:
                        pc.printf("Unknown Error. Message: %x\n\r", pMessage->message);
                        break;
                }
                mailStack.free(pMessage);
            }
        } 

        
        //TODO: stop function, maybe use parent deconstructor
        //void stop_comm{}

    public: 
    
        Comm(): pc(SERIAL_TX, SERIAL_RX), T_(){ // inherit from the RawSerial constructor
            
            MAXCMDLENGTH = 18;
            newCmd[MAXCMDLENGTH] = '0';
            cmdIndx = 0;

            motorPower = 300;
            targetVel = 45.0;
            targetRot = 459.0;
        }
        
        
        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(){
            p_comm_in->start(callback(this, &Comm::commInFn));
            p_comm_out->start(callback(this, &Comm::commOutFn));

            _RUN = true;
        }
};


//Main
int main() {

   // std::ios::sync_with_stdio(false);
    SHA256::SHA256 Miner;
    
    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;
    Timer timer;   
    
    // Motor States
    int8_t orState = 0;    //Rotot offset at motor state 0
    int8_t currentState = 0;    //Rotot offset at motor state 0
    int8_t stateList[6];    //Rotot offset at motor state 0
    //Run the motor synchronisation


    Motor motor;
    Motor* p_motor = &motor;
    orState = p_motor->motorHome();
    
    // Add callbacks
   // I1.fall(&stateUpdate);
   // I2.fall(&stateUpdate);
   // I3.fall(&stateUpdate);
    int8_t* params[2];
    params[0] = &currentState;
    params[1] = &orState;
    
    I1.fall(callback(*(p_motor->stateUpdate),params));
    I2.fall(callback(&stateUpdate,params));
    I3.fall(callback(&stateUpdate,params));
    
    I1.rise(callback(&stateUpdate,params));
    I2.rise(callback(&stateUpdate,params));
    I3.rise(callback(&stateUpdate,params));
    
    // Push motor to move
    currentState = readRotorState();
    motorOut((currentState-orState+lead+6)%6); // We push it digitally
    
    pc.printf("Rotor origin: %x\n\r",orState);
    orState is subtracted from future rotor state inputs to align rotor and motor states
    intState = readRotorState();
    if (intState != intStateOld) {
       pc.printf("old:%d \t new:%d \t next:%d \n\r",intStateOld, intState, (intState-orState+lead+6)%6);
       intStateOld = intState;
       motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
   }


    // Keep the program running indefinitely
    timer.start();          // start timer
    int stateCount = 0;
    while (1) {
       pc.printf("Current:%d \t Next:%d \n\r", currentState, (currentState-orState+lead+6)%6);
        Miner.computeHash(hash, sequence, length64);
        hashCounter++;
        if ((hash[0]==0) && (hash[1]==0)){
            //pc.printf("hash: ");
            //for(int i = 0; i < 32; ++i)
                //pc.printf("%02x", hash[i]);
            //pc.printf("\n\r");
        }

        // // Try a new nonce
        (*nonce)++;

        if (stateCount<6){
            stateList[stateCount] = currentState;
            stateCount++;
        }
        else {
            //pc.printf("states");
            //for(int i = 0; i < 6; ++i)
                //pc.printf("%02i,", stateList[i]);
            //pc.printf("\n\r");    
            stateCount = 0;        
        }

        // // Per Second i.e. when greater or equal to 1
        if (timer.read() >= 1){
            //pc.printf("HashRate = %02u \n\r",hashCounter);
            hashCounter=0;
            timer.reset();
        }
    }
  
}