Callum and Adel's changes on 12/02/19

Dependencies:   Crypto

main.cpp

Committer:
adehadd
Date:
2019-03-22
Revision:
49:ae8dedfe2d0f
Parent:
48:b2afe48ced0d
Child:
50:d1b983a0dd6f

File content as of revision 49:ae8dedfe2d0f:

/*TODO:
Change:
    Indx
    _newCmd
    _MAXCMDLENGTH
move the global variables to a class because we arent paeasents - Mission Failed
use jack's motor motor position
fix class variable naming
dont make everything public becuase thats fucling dumb and defeats the whole point of a class

Move things out of public and into a protected part of the class

Move char Comm::_inCharQ[] = {'.','.','... into the class by making it a vector

Change abs lacro to 
NOT V0 but R0 (to go on forever)

Actually make the code robust lol
*/

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

//Header Files
#include "SHA256.h"
#include "mbed.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 High Pins                                                 //Mask in output byte
#define L1Hpin A3                                                       //0x02
#define L2Hpin A6                                                       //0x08
#define L3Hpin D2                                                       //0x20

//Motor Drive Low Pins 
#define L1Lpin D1                                                       //0x01
#define L2Lpin D0                                                       //0x04
#define L3Lpin D10                                                      //0x10

//Motor Pulse Width Modulation (PWM) Pin  
#define PWMpin D9

//Motor current sense
#define MCSPpin A1
#define MCSNpin A0

// "Lacros" for utility
#define max(x,y) ( (x)>=(y) ? (x):(y) )
#define min(x,y) ( (x)>=(y) ? (y):(x) )
#define sgn(x)   ( (x)>= 0  ?  1 :-1  )

//Status LED
DigitalOut led1(LED1);

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

//Motor Drive High Outputs
DigitalOut L1H(L1Hpin);
DigitalOut L2H(L2Hpin);
DigitalOut L3H(L3Hpin);

//Motor Drive Low Outputs
DigitalOut L1L(L1Lpin);
DigitalOut L2L(L2Lpin);
DigitalOut L3L(L3Lpin);

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

#ifndef     MAXCMDLENGTH
    #define MAXCMDLENGTH 18
#endif

#ifndef     MAXCMDLENGTH_HALF
    #define MAXCMDLENGTH_HALF 9
#endif

class Comm{

    public:

        volatile bool       _outMining;
        volatile float      _targetVel, _targetRot;
        volatile char       _notes[MAXCMDLENGTH_HALF];                  // Array of actual _notes 

        volatile int8_t     _modeBitField;                              // 0,0,0,... <=> Melody,Torque,Rotation,Velocity
        const    uint8_t    _MAXCMDLENGTH;                              // 
        volatile uint8_t    _inCharIndex, _cmdIndex,
                            _noteDur[MAXCMDLENGTH_HALF],_noteLen;       // Array of note durations
        volatile uint32_t   _motorTorque;                               // Motor Toque
        volatile uint64_t   _newKey;                                    // hash key

        
        char _inCharQ[MAXCMDLENGTH],
             _newCmd[MAXCMDLENGTH];
        
        RawSerial           _pc;
        Thread              _tCommOut, _tCommIn;
        Mutex               _newKeyMutex;                               // Restrict access to prevent deadlock.
        bool                _RUN;
        
        enum msgType      { mot_orState, posIn, velIn, posOut, velOut,
                            hashRate, keyAdded, nonceMatch,
                            torque, rotations, melody,
                            error};

        typedef struct    { msgType type;
                            uint32_t message;} msg; // TODO: Maybe add a thing that stores the newCmd message as well

        Mail<msg, 32>       _msgStack;
        Mail<bool,32>       _msgReceived;


        //-- Default Constructor With Inheritance From RawSerial Constructor ------------------------------------------------//
        Comm(): _pc(SERIAL_TX, SERIAL_RX), _tCommOut(osPriorityNormal, 1024), _tCommIn(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(MAXCMDLENGTH){

            _cmdIndex     = 0;
            _inCharIndex  = 0;

            _outMining    = false;
            _motorTorque  = 300;
            _targetRot    = 459.0;
            _targetVel    = 45.0;

            _modeBitField = 0x01;                                       // Default velocity mode

            _pc.printf("\n\r%s %d\n\r>", "Welcome", _MAXCMDLENGTH );                    // Welcome                                       
            for (int i = 0; i < _MAXCMDLENGTH; ++i)                     // Reset buffer
                _inCharQ[i] = (char)'.';                                // If a null terminator is printed Mbed prints 'Embedded Systems are fun and do awesome things!'

            _inCharQ[_MAXCMDLENGTH] = (char)'\0';
            sprintf(_inCharQ, "%s", _inCharQ);                          // Handling of the correct string
            strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH);

            _pc.printf("%s\n\r<", _inCharQ);
            
        }

        void commInFn() {
            _pc.attach(callback(this, &Comm::serialISR));
            char newChar;

            while (_RUN) {
                osEvent newEvent = _msgReceived.get();                  // Waits forever until it receives a thing
                _msgReceived.free((bool *)newEvent.value.p);
                
                if (_inCharIndex == (_MAXCMDLENGTH)) {
                    _inCharQ[_MAXCMDLENGTH] = '\0';                      // Force the string to have an end character
                    putMessage(error, 1);
                    _inCharIndex = 0;                                   // Reset buffer index
                }
                else{
                    newChar = _inCharQ[_inCharIndex];

                    if(newChar != '\r'){               // While the command is not over,
                        _inCharIndex++;                                 // Advance index
                        _pc.putc(newChar);
                    }
                    else{
                        _inCharQ[_inCharIndex] = '\0';                   // When the command is finally over,
                        strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH);        // Will copy 18 characters from _inCharQ to _newCmd

                        for (int i = 0; i < _MAXCMDLENGTH; ++i)         // Reset buffer
                            _inCharQ[i] = ' ';
                        
                        _inCharIndex = 0;                               // Reset index  
                        cmdParser();

                        // _tCommIn.signal_wait(0x01);
                    }
                }
            }
        }

        //-- Interrupt Service Routine for Serial Port and Character Queue Handling -----------------------------------------//
        void serialISR() {
            if (_pc.readable()) {
                char newChar = _pc.getc();
                _inCharQ[_inCharIndex] = newChar;                       // Save input character

                bool *new_msg = _msgReceived.alloc();
                *new_msg = true;
                _msgReceived.put(new_msg);
            }
        }

        //-- Reset Cursor Position ------------------------------------------------------------------------------------------//
        void returnCursor() {
            _pc.putc('>');
            for (int i = 0; i < _inCharIndex; ++i)                  
                _pc.putc(_inCharQ[i]);
        }
    
        //-- Parse Incoming Data From Serial Port ---------------------------------------------------------------------------//
        void cmdParser() {
            switch(_newCmd[0]) {
                case 'K':                                               // keyAdded
                    _newKeyMutex.lock();                                // Ensure there is no deadlock
                    sscanf(_newCmd, "K%x", &_newKey);                    // Find desired the Key code
                    putMessage(keyAdded, _newKey);                      // Print it out
                    _newKeyMutex.unlock();
                    break;

                case 'V':                                               // velIn
                    sscanf(_newCmd, "V%f", &_targetVel);                 // Find desired the target velocity
                    _modeBitField = 0x01;                               // Adjust bitfield pos 1
                    putMessage(velIn, _targetVel);                      // Print it out
                    break;

                case 'R':                                               // posIn
                    sscanf(_newCmd, "R%f", &_targetRot);                 // Find desired target rotation
                    _modeBitField = 0x02;                               // Adjust bitfield pos 2
                    putMessage(posIn, _targetRot);                      // Print it out
                    break;

                case 'x':                                               // Torque
                    sscanf(_newCmd, "x%u", &_motorTorque);               // Find desired target torque
                    _modeBitField = 0x04;                               // Adjust bitfield pos 3
                    putMessage(torque, _motorTorque);                   // Print it out
                    break;

                case 'M':                                               // Mining display toggle
                    int8_t miningTest;
                    sscanf(_newCmd, "M%d", &miningTest);                 // Display if input is 1
                    miningTest == 1 ? _outMining = true : _outMining = false;
                    break;

                case 'T':                                               // Play tune
                    regexTune() ? putMessage(melody, 1) : putMessage(error, 2); 
                    break;                                              // Break from case 'T'

                default:                                                // Break from switch
                    break;
            }
        }

        //-- Read In Note Data From Serial Port and Parse Into Class Variables ----------------------------------------------//
        bool regexTune() {

            uint8_t len = 0;
                                   
            for (int i = 1; i < _MAXCMDLENGTH; ++i)                     // Find first #
                if (_newCmd[i] == '#') {
                    len = i;
                    break;                                              // Stop at first # found
                }

            if (len>0) {                                                // Parse the input only if # found
                uint8_t newLen = 2*(len+1)+1;
                bool isChar = true;
                char formatSpec[newLen];
                formatSpec[0]='T';
                for (int i = 1; i < newLen; i=i+2) {                    // Create a format spec based on length of input
                    formatSpec[i] = '%';
                    isChar ? formatSpec[i+1] = 'c' : \
                             formatSpec[i+1] = 'u' ;
                    isChar = !isChar;
                }

                formatSpec[newLen] = '\0';
                sprintf(formatSpec, "%s", formatSpec);                  // Set string format correctly
                // _pc.printf("%s\n", formatSpec );
                sscanf(_newCmd, formatSpec, &_notes[0], &_noteDur[0],
                                           &_notes[1], &_noteDur[1],
                                           &_notes[2], &_noteDur[2],
                                           &_notes[3], &_noteDur[3],
                                           &_notes[4], &_noteDur[4],
                                           &_notes[5], &_noteDur[5],
                                           &_notes[6], &_noteDur[6],
                                           &_notes[7], &_noteDur[7],
                                           &_notes[8], &_noteDur[8]); 
                
                         
                // Update _newCmd for putMessage print
                sprintf(_newCmd,formatSpec, _notes[0],  _noteDur[0],\
                                            _notes[1],  _noteDur[1],\
                                            _notes[2],  _noteDur[2],\
                                            _notes[3],  _noteDur[3],\
                                            _notes[4],  _noteDur[4],\
                                            _notes[5],  _noteDur[5],\
                                            _notes[6],  _noteDur[6],\
                                            _notes[7],  _noteDur[7],\
                                            _notes[8],  _noteDur[8]);
                _noteLen = len;
                return true;
            }
            else {
                return false;
            }   
        } 

        //-- Decode Messages to Print on Serial Port ------------------------------------------------------------------------//
        void commOutFn() {
            while (_RUN) {

                osEvent newEvent = _msgStack.get();
                msg *pMessage = (msg *) newEvent.value.p;

                //Case switch to choose serial output based on incoming message enum
                switch (pMessage->type) {
                    case mot_orState:
                        _pc.printf("\r>%s< The motor is currently in state %x\n\r", _inCharQ, pMessage->message);
                        break;
                    case hashRate:
                        if (_outMining) {
                            _pc.printf("\r>%s< Mining: %.4u Hash/s\r",              _inCharQ, (uint32_t) pMessage->message);
                            returnCursor();
                            _outMining = false;
                        }
                        break;
                    case nonceMatch:
                        _pc.printf("\r>%s< Nonce found: %x\n\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, (int32_t) 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 States/sec\n\r", _inCharQ, (float) ((int32_t) pMessage->message));
                        break;
                    case posIn:
                        _pc.printf("\r>%s< Target # Rotations:\t%.2f\n\r",          _inCharQ, (float) ((int32_t) pMessage->message));
                        break;
                    case posOut:
                        _pc.printf("\r>%s< Current Position:\t%.2f\n\r",            _inCharQ, (float) ((int32_t) pMessage->message));
                        break;
                    case melody:
                        _pc.printf("\r>%s< New Tune:\t%s\n\r",                      _inCharQ, _newCmd);
                        break;  
                    case error:
                        switch (pMessage->message) {
                            case 1:
                                _pc.printf("\r>%s< Error:%s\n\r",                   _inCharQ, "Overfull Buffer Reset" );
                                break;
                            case 2:
                                _pc.printf("\r>%s< Error:%s\n\r",                   _inCharQ, "Invalid Melody" );
                            default:
                                break;
                        }
                        for (int i = 0; i < _MAXCMDLENGTH; ++i)         // reset buffer
                            _inCharQ[i] = ' ';

                        _inCharIndex = 0;
                        break;
                    default:
                        _pc.printf("\r>%s< Unknown Error. Message: %x\n\r", _inCharQ, pMessage->message);
                        break;
                }

                _msgStack.free(pMessage);
            }
        }

        //-- Put a Message On the Outgoing Message Stack --------------------------------------------------------------------//
        void putMessage(msgType type, uint32_t message){
            msg *p_msg = _msgStack.alloc();
            p_msg->type = type;
            p_msg->message = message;
            _msgStack.put(p_msg);
        }

        //-- Attach CommOut Thread to the Outgoing Communication Function ---------------------------------------------------//
        void start_comm(){
            _RUN = true;
            _tCommOut.start(callback(this, &Comm::commOutFn));
            _tCommIn.start(callback(this, &Comm::commInFn));
        }

        
};
// char Comm::_inCharQ[] = {'.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','\0'}; // Static member must be defined outside class
//Mutex Comm::_newKeyMutex;

class Motor {

    protected:
        volatile int8_t _orState,                                       // Rotor offset at motor state 0, motor specific
                        _currentState,                                  // Current Rotor State
                        _stateList[6],                                  // All possible rotor states stored
                        _lead;                                          // Phase _lead to make motor spin

        uint8_t         _theStates[3],                                  // The Key states
                        _stateCount[3];                                 // State Counter
        uint32_t        mtrPeriod,                                      // Motor period
                        _MAXPWM_PRD;
        float           _dutyC;                                         // 1 = 100%
        bool            _RUN;

        Comm*           _pComm;
        Thread          _tMotorCtrl;                                    // Thread for motor Control

    public:
        //-- Default Constructor With Thread Object Constructor -------------------------------------------------------------//
        Motor() : _tMotorCtrl(osPriorityAboveNormal2, 1024){

            _dutyC         = 1.0f;                                      // Set Power to maximum to drive motorHome()
            mtrPeriod      = 2e3;                                       // Motor period
            pwmCtrl.period_us(mtrPeriod);                               // Initialise PWM
            pwmCtrl.pulsewidth_us(mtrPeriod);

            _orState       = motorHome();                               // Rotor offset at motor state 0
            _currentState  = readRotorState();                         // Current Rotor State
            _lead = 2;                                                  // 2 for forwards, -2 for backwards

            _theStates[0]  = _orState +1;                               // Initialise repeatable states, for us this is state 1
            _theStates[1]  = (_orState + _lead) % 6 +1;                 // state 3
            _theStates[2]  = (_orState + (_lead*2)) % 6 +1;             // state 5

            _stateCount[0] = 0;                                         // Initialise
            _stateCount[1] = 0; 
            _stateCount[2] = 0;

            _pComm         = NULL;                                      // Initialise as NULL
            _RUN           = false;

            _MAXPWM_PRD = 2e3;

        }

        //-- Start Motor and Attach State Update Function to Rise/Fall Interrupts -------------------------------------------//
        void motorStart(Comm *comm) {

            I1.fall(callback(this, &Motor::stateUpdate));               // Establish Photo Interrupter Service Routines (auto choose next state)
            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));

            motorOut((_currentState-_orState+_lead+6)%6);               // Push digitally so static motor will start moving

            
            _dutyC = 0.8;                                               // Default a lower duty cycle
            pwmCtrl.period_us((uint32_t)mtrPeriod);
            pwmCtrl.pulsewidth_us((uint32_t)mtrPeriod*_dutyC);

             _pComm = comm;
            _RUN = true;

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

            _pComm->_pc.printf("origin=%i, _theStates=[%i,%i,%i]\n\r", _orState, _theStates[0], _theStates[1], _theStates[2]); // Print information to terminal

        }

        //-- Set a Predetermined Drive State  -------------------------------------------------------------------------------//
        void motorOut(int8_t driveState) {

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

            if (~driveOut & 0x01) L1L = 0;                              //Turn off first
            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;

            if (driveOut  & 0x01) L1L = 1;                              //Then turn on
            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;
        }

        //-- Inline Conversion of Photointerrupts to a Rotor State ----------------------------------------------------------//
        inline int8_t readRotorState() {
            return stateMap[I1 + 2*I2 + 4*I3];
        }

        //-- Basic Motor Stabilisation and Synchronisation ------------------------------------------------------------------//
        int8_t motorHome() {

            motorOut(0);                                                //Put the motor in drive state 0 and wait for it to stabilise
            wait(3.0);

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

        //-- Motor State Log, Circumvents Issues From Occasionally Skipping Certain States ----------------------------------//
        void stateUpdate() { // () { // **params
            _currentState = readRotorState();                          // Get current state

            if (_currentState == _theStates[0])                         // Cumulative state counter (1 for our group's motor)
                _stateCount[0]++;
            else if (_currentState == _theStates[1])                    // Cumulative state counter (3 for our group's motor)
                _stateCount[1]++;
            else if (_currentState == _theStates[2])                    // Cumulative state counter (5 for our group's motor)
                _stateCount[2]++;

            motorOut((_currentState - _orState + _lead + 6) % 6);       // Send the next state to the motor 

        }

        //-- Motor PID Control  ---------------------------------------------------------------------------------------------//
        void motorCtrlFn() {
            Ticker motorCtrlTicker;
            Timer  m_timer;
            motorCtrlTicker.attach_us(callback(this,&Motor::motorCtrlTick), 1e5);

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

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

            int32_t          cur_speed;                                 // Variable for local velocity calculation
            int32_t          locMotorPos;                               // Local copy of motor position
            volatile int32_t torque;                                    // Local variable to set motor torque
            static int32_t   oldTorque =0;
            float            sError,                                    // Velocity error between target and reality
                             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
            int32_t          Ki = 12;
            float            Kd=15.5;


            int32_t          Ts;                                        // Initialise controller output Ts  (s=speed)
            int32_t          Tr;                                        // Initialise controller output Tr (r=rotations)

            int32_t          old_pos = 0;

            uint32_t         cur_time = 0,
                             old_time = 0,
                             time_diff;

            float            cur_err = 0.0f,
                             old_err = 0.0f,
                             err_diff;

            m_timer.start();

            while (_RUN) {
                _tMotorCtrl.signal_wait((int32_t)0x1);
                // _pComm->_tCommIn.signal_set(0x01);

                core_util_critical_section_enter();                     // Access shared variables here
                    cpyModeBitfield = _pComm->_modeBitField;
                    std::copy(_stateCount, _stateCount+3, cpyStateCount);
                    cpyCurrentState = _currentState;
                    for (int i = 0; i < 3; ++i) {
                        _stateCount[i] = 0;
                    }
                core_util_critical_section_exit();

                
                cur_time = m_timer.read();                              // Read state & timestamp

                time_diff = cur_time - old_time;
                // cur_speed = (cur_pos - old_pos) / time_diff;
     
                old_time = cur_time;                                    // prep values for next time through loop
                old_pos  = cpyCurrentState;

                // Hence we make the value positive,// and instead set the direction to the opposite one

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

                totalDegrees = ting[0] * cpyStateCount[iterElementMax];
                stateDiff = _theStates[iterElementMax]-cpyCurrentState;
                stateDiff >= 0 ? totalDegrees = totalDegrees + (ting[1]* stateDiff)    : \
                                 totalDegrees = totalDegrees + (ting[1]* stateDiff *-1);

                if ((cpyModeBitfield & 0x01)|(cpyModeBitfield & 0x02)) {// Speed, torque control and PID
                    cur_speed = totalDegrees / time_diff;
                    sError = (_pComm->_targetVel * 6) - abs(cur_speed); // Read global variable _targetVel updated by interrupt and calculate error between target and reality

                    // Ts = Kp * (s -|v|) where,                        // SPEED CONTROLLER
                    // Ts = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity

                    // Check if user entered V0 and set the output to maximum as specified
                    sError == -abs(cur_speed) ? Ts = _MAXPWM_PRD : \
                                                Ts = (Kp1 * sError);    // If the user didn't enter V0 implement controller transfer function: 
                                                                        
                                                                                                                  
                    // Tr= Kp*Er + Kd* (dEr/dt) where,                  // ROTATION CONTROLLER
                                                                        // Tr = controller output, Kp = prop controller constant, Er = error in number of rotations
                    rError = (_pComm->_targetRot)*6 - totalDegrees;     // Read global variable _targetRot updated by interrupt and calculate the rotation error.
                    Tr = Kp2*rError + Kd*(rError - rErrorOld);          // Implement controller transfer function 
                    rErrorOld = rError;                                 // Update rotation error
                                                                        
                    Ts = (int32_t)( Ts * sgn(rError) );                 // Use the sign of the error to set controller wrt direction of rotation

                    cur_speed < 0 ? torque = max(Ts, Tr): torque = min(Ts, Tr);

                }
                else if (cpyModeBitfield & 0x04) {                      // If it is in torque mode, do no PID math, just set pulsewidth
                    torque = (int32_t)_pComm->_motorTorque;
                    if (oldTorque != torque) {
                        _pComm->putMessage((Comm::msgType)8, torque);
                        oldTorque = torque;
                    }
                }
                else{
                    torque = _MAXPWM_PRD * 0.5;                         // Run at 50% duty cycle if argument not properly defined

                }

                torque    < 0 ? _lead   = -2         : _lead   = +2;
                torque = abs(torque);

                if(torque > _MAXPWM_PRD) torque = _MAXPWM_PRD;          // In case the calculated PWM is higher than our maximum 50% allowance,
                                                                        // Set it to our max.
                _pComm->_motorTorque = torque;
                pwmCtrl.pulsewidth_us(torque);

            }
        }

        void motorCtrlTick(){
            _tMotorCtrl.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};
    uint8_t   hash[32];
    uint32_t  length64 = 64;
    uint32_t  hashCounter = 0;
    uint64_t* nonce = (uint64_t*)((int)sequence + 56);
    uint64_t* key = (uint64_t*)((int)sequence + 48);

    // Begin Main Timer
    Timer timer;
    timer.start();

    // Loop Program 
    while (1) {

        //try{

            // Mutex For Access Control
            comm_port._newKeyMutex.lock();
            *key = comm_port._newKey;
            comm_port._newKeyMutex.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();
            }
        //}

        //catch(...){
        //  break;
        //}

    }

    return 0;
}