Callum and Adel's changes on 12/02/19
Dependencies: Crypto
main.cpp
- Committer:
- iachinweze1
- Date:
- 2019-03-22
- Revision:
- 53:89d16b398615
- Parent:
- 51:c03f63c6f930
- Child:
- 54:bd5b586aa2e1
File content as of revision 53:89d16b398615:
//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); //Encoder inputs InterruptIn CHA(CHApin); InterruptIn CHB(CHBpin); //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, _noteRep, _noteDur[MAXCMDLENGTH_HALF],_noteLen; // Array of note durations volatile uint32_t _motorTorque; // Motor Toque volatile uint64_t _newKey; // hash key volatile int32_t _motor_pos; 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; _motor_pos = 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 _motor_pos = 0; if (&_targetVel < 0) { _targetRot = -100; } else { _targetRot = 100; } 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 _targetVel = 2e3; _motor_pos = 0; 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 specLen = 2*(len+1) +1; // Add extra character for number of repeats, and +1 for the letter T bool isChar = true; // After 'T' first is character note char formatSpec[specLen]; formatSpec[0]='T'; for (int i = 1; i < specLen; 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[specLen] = '\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-1)/2; _modeBitField = 0x08; _noteRep = _noteDur[(len-1)/2]; 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 int8_t old_rotor_state; uint8_t encState; uint32_t quadratureStates; uint32_t MINPWM_PRD; uint32_t maxEncCount; uint32_t encCount; uint32_t encTotal; uint32_t badEdges; public: //-- Default Constructor With Thread Object Constructor -------------------------------------------------------------// Motor() : _tMotorCtrl(osPriorityAboveNormal2, 2048){ _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 old_rotor_state = _orState; // Set old_rotor_state to the origin to begin with _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; maxEncCount = 0; encCount = 0; encTotal = 0; badEdges = 0; quadratureStates = 1112; } int findMinTorque() { int8_t prevState = readRotorState(); uint32_t _mtPeriod = 700; Timer testTimer; testTimer.start(); _pComm->_pc.printf("PState:%i, CState:%i\n", prevState, readRotorState()); // stateUpdate(); while (readRotorState() == prevState) { testTimer.reset(); // pwmCtrl.period_us(2e3); pwmCtrl.pulsewidth_us(_mtPeriod); stateUpdate(); while (testTimer.read_ms() < 500) {} // prevState = readRotorState(); _mtPeriod += 10; _mtPeriod = _mtPeriod >= 1000 ? 700 : _mtPeriod; } _pComm->_pc.printf("Min Torque:%i\n", _mtPeriod); return _mtPeriod; } //-- 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; MINPWM_PRD = 920; _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 motorOut((_currentState - _orState + _lead + 6) % 6); // Send the next state to the motor if (_currentState == 1) { //if (encCount > maxEncCount) maxEncCount = encCount; //if (encCount < minEncCount) minEncCount = encCount; //if (badEdges > maxBadEdges) maxBadEdges = badEdges; //revCount++; encTotal += encCount; encCount = 0; badEdges = 0; } if (_currentState - old_rotor_state == 5) { _pComm->_motor_pos--; } else if (_currentState - old_rotor_state == -5) { _pComm->_motor_pos++; } else { _pComm->_motor_pos += (_currentState - old_rotor_state); } old_rotor_state = _currentState; } // A Rise void encISR0() { if (encState == 3) {encCount++;} else badEdges++; encState = 0; } // B Rise void encISR1() { if (encState == 0) {encCount++;} else badEdges++; encState = 1; } // A Fall void encISR2() { if (encState == 1) {encCount++;} else badEdges++; encState = 2; } // B Fall void encISR3() { if (encState == 2) {encCount++;} else badEdges++; encState = 3; } //-- 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, eError = 0; // Rotation error between target and reality static float rErrorOld; // Old rotation error used for calculation float encRemain = 0; bool attached = true; bool declared = false; //~~~Controller constants~~~~ int32_t Kp1=88; //22, 27 //Proportional controller constants int32_t Kp2=26; //12 //Calculated by trial and error to give optimal accuracy float Kd =34.5; // 40, 14.75 float Kis = 10.0f; // 50 int32_t Kir = 0.0f; float sIntegral = 0.0f; float rIntegral = 0.0f; int32_t Ts; // Initialise controller output Ts (s=speed) int32_t Tr; // Initialise controller output Tr (r=rotations) int32_t old_pos = 0, cur_pos = 0, sign = 1; float cur_err = 0.0f, old_err = 0.0f, err_diff, time_diff, hold_pos = 0, cur_time = 0.0f, old_time = 0.0f, cur_speed = 0.0f; enum { // To nearest Hz NOTE_C = 261, // C4 NOTE_D = 293, // D4 NOTE_E = 330, // E4 NOTE_F = 349, // F4 NOTE_G = 392, // G4 NOTE_A = 440, // A4 NOTE_B = 494 // B4 }; m_timer.start(); while (_RUN) { _tMotorCtrl.signal_wait((int32_t)0x1); core_util_critical_section_enter(); // Access shared variables here cpyModeBitfield = _pComm->_modeBitField; core_util_critical_section_exit(); if ((cpyModeBitfield & 0x01) | (cpyModeBitfield & 0x02)) {// Speed, torque control and PID if (abs(cur_speed) <= 120 && (!attached)) { eError = 1.0f; attached = true; CHA.rise(callback(this, &Motor::encISR0)); CHB.fall(callback(this, &Motor::encISR3)); CHB.rise(callback(this, &Motor::encISR1)); CHA.fall(callback(this, &Motor::encISR2)); encCount = 0; encTotal = 0; encRemain = old_err * quadratureStates; // Number of encs remaining } else if (abs(cur_speed) > 120 && (attached)) { eError = 0.0f; attached = false; CHA.rise(NULL); CHB.fall(NULL); CHB.rise(NULL); CHA.fall(NULL); encCount = 0; encTotal = 0; eError = 0; // Store the number of rotations that have been done up to the point where the channels are activated hold_pos = (cur_pos / 6.0f); _pComm->_pc.printf("HPos:%f\n", hold_pos); } // read state & timestamp if (cur_pos < 2) { rIntegral = 0.0f; sIntegral = 0.0f; sign = sgn(_pComm->_targetRot); } cur_pos = (_pComm->_motor_pos)*sign; // USE cur_time = m_timer.read(); // compute speed time_diff = cur_time - old_time; cur_speed = (cur_pos - old_pos) / time_diff; // prep values for next time through loop old_time = cur_time; old_pos = cur_pos; // Position error // if (!attached) { rError = (_pComm->_targetRot) - (cur_pos/6.0f); // } // else{ // rErrorOld = RError; // rError = (_pComm->_targetRot) - (hold_pos + ((encRemain - encTotal)/quadratureStates)); // } if ((cur_speed != 0)) { rIntegral += rError * time_diff; } err_diff = rError - old_err; old_err = rError; // Speed error - Convert curr_speed from states per time to rotations per time // TODO: Check the direction that CPos goes in when _targetVel < 0 sError = (_pComm->_targetVel) - (abs(cur_speed/6.0f)); //Read global variable _targetVel updated by interrupt and calculate error between target and reality if ((cur_speed != 0) && (torque != _MAXPWM_PRD)) { sIntegral += sError * time_diff; if (abs(sIntegral * Kis) >= 2000) { sIntegral -= sError * time_diff; } } Ts = (int32_t)(((Kp1 * sError * sign) + (Kis * sIntegral * sign))); // * sgn(cur_pos)); Tr = (int32_t)((Kp2*rError*sign) + (Kd/time_diff) * err_diff * sign); if (cpyModeBitfield & 0x01) { // Speed control if (_pComm->_targetVel == 0) { //Check if user entered V0, torque = _MAXPWM_PRD; //and set the output to maximum as specified } else { // select minimum absolute value torque // if (cur_speed < 0) { // torque = max(Ts, Tr); // } // else { // torque = min(Ts, Tr); // } torque = Ts; if (abs(sError) > 0.6) { // torque = abs(Tr) < 1000 ? 1000*sgn(Tr) : Tr; torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque; // _pComm->_pc.printf("Speed:%f\r\n", (cur_speed/6.0f)); } else { torque = 0; } torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque; } } else if (cpyModeBitfield & 0x02) { // select minimum absolute value torque if (_pComm->_targetRot == 0) { // Not spinning at max speed torque = 0.7 * _MAXPWM_PRD; } else { // select minimum absolute value torque // if (cur_speed < 0) { // torque = max(Ts, Tr); // } else { // torque = min(Ts, Tr); // } torque = Tr; if (abs(rError) > 0.3) { torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque; } else { torque = 0; if (!declared) { _pComm->_pc.printf("Remaining Turns:%f\n", rError); declared = true; } //attached = false; } } } if (torque < 0) { torque = -torque; _lead = -2; } else { _lead = 2; } torque = torque > _MAXPWM_PRD ? _MAXPWM_PRD : torque; // Set a cap on torque _pComm->_motorTorque = torque; pwmCtrl.pulsewidth_us(torque); // _pComm->_pc.printf("EError:%.4f, tot:%d, encRemain:%.4f || RError:%.4f || SError:%.4f, CSpeed:%f, Torque:%i\r\n", eError, encTotal, encRemain, rError, sError, (cur_speed/6.0f), torque); //_pComm->_pc.printf("EError:%.4f, remain:%.4f, tot%d || RError:%.4f || Torque:%i \r\n", eError, encRemain, encTotal, rError, torque); _pComm->_pc.printf("RError:%.4f\r\n", rError); // Give the motor a kick stateUpdate(); } 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 if (cpyModeBitfield & 0x08) { // If it is in Melody mode uint32_t oldMtrPeriod = _mtrPeriod; // Backup of current motor period float oldDutyC = _dutyC; // Backup of current duty cycle uint32_t noteFreq, newDur, newPeriod =0, oldPeriod = _mtrPeriod; Timer testTimer; testTimer.start(); _pComm->_pc.printf("rep:%i, len:%i \n\r", _pComm->_noteRep, _pComm->_noteLen); for (int k = 0; k < _pComm->_noteRep; ++k) { for (int i = 0; i < _pComm->_noteLen; ++i) { noteFreq = 0; newDur = (uint32_t)_pComm->_noteDur[i]*1e3; // ms switch (_pComm->_notes[i]) { case 'C': noteFreq = NOTE_C; break; case 'D': noteFreq = NOTE_D; break; case 'E': noteFreq = NOTE_E; break; case 'F': noteFreq = NOTE_F; break; case 'G': noteFreq = NOTE_G; break; case 'A': noteFreq = NOTE_A; break; case 'B': noteFreq = NOTE_B; break; default: break; } if (noteFreq == 0) {break;} // leave the loop if we get a wrong character newPeriod = (uint32_t)(1e6/(uint32_t)noteFreq); // us if (newPeriod>oldPeriod) { // Change Period First _pComm->_pc.printf("Period:changed \n" ); pwmCtrl.period_us(newPeriod); //set frequency of PWM pwmCtrl.pulsewidth_us((uint32_t)(newPeriod*0.8)); } else { // Change Pulse WidthFirst pwmCtrl.pulsewidth_us((uint32_t)(newPeriod*0.8)); pwmCtrl.period_us(newPeriod); //set frequency of PWM } // stateUpdate(); oldPeriod = newPeriod; testTimer.reset(); while (testTimer.read_ms() < newDur ) {} // Do nothing _pComm->_pc.printf("Period:%d Dur:%d \n",newPeriod, newDur ); } _pComm->_modeBitField = 0x04; // stop melody mode } // After playing notes, go back to the old speed torque = (int32_t)_pComm->_motorTorque; pwmCtrl.pulsewidth_us((uint32_t)(torque*0.8)); // And reset the motor period and duty cycle } else{ torque = _MAXPWM_PRD * 0.5; // Run at 50% duty cycle if argument not properly defined } } } 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; }