![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
main.cpp
- Committer:
- CallumAlder
- Date:
- 2019-03-21
- Revision:
- 48:b2afe48ced0d
- Parent:
- 47:21bf4096faa1
- Child:
- 49:ae8dedfe2d0f
File content as of revision 48:b2afe48ced0d:
/*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 Mutex _newKeyMutex; // Restrict access to prevent deadlock. bool YES = false; class Comm{ public: volatile bool _outMining; volatile float _targetVel, _targetRot; volatile char _notes[9]; // 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[9],_noteLen; // Array of note durations volatile uint32_t _motorTorque; // Motor Toque volatile uint64_t _newKey; // hash key RawSerial _pc; Thread _tCommOut; 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; Mail<msg, 32> _msgStack; //-- Default Constructor With Inheritance From RawSerial Constructor ------------------------------------------------// Comm(): _pc(SERIAL_TX, SERIAL_RX), _tCommOut(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(18){ _cmdIndex = 0; _inCharIndex = 0; _outMining = false; _motorTorque = 300; _targetRot = 459.0; _targetVel = 45.0; _modeBitField = 0x01; // Default velocity mode _pc.printf("\n\r%s\n\r>", "Welcome" ); // 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); _pc.putc('<'); _pc.attach(callback(this, &Comm::serialISR)); } //-- Interrupt Service Routine for Serial Port and Character Queue Handling -----------------------------------------// void serialISR() { if (_pc.readable()) { char newChar = _pc.getc(); if (_inCharIndex == (_MAXCMDLENGTH)) { _inCharQ[_MAXCMDLENGTH] = '\0'; // Force the string to have an end character putMessage(error, 1); _inCharIndex = 0; // Reset buffer index } else{ if(newChar != '\r'){ // While the command is not over, _inCharQ[_inCharIndex] = newChar; // Save input character and _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 YES = true; // cmdParser(); // Parse the command for decoding } } } } //-- 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) { if (YES){ // waiiiiiittt cmdParser(); // Parse the command for decoding 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); YES = false; } } } //-- 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)); } char _newCmd[]; // Unallocated must be defined at the bottom of the class static char _inCharQ[]; }; 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 = readRot_orState(); // 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 readRot_orState() { 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 readRot_orState(); //Get the rotor state } //-- Motor State Log, Circumvents Issues From Occasionally Skipping Certain States ----------------------------------// void stateUpdate() { // () { // **params _currentState = readRot_orState(); // 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); 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 _newKeyMutex.lock(); *key = comm_port._newKey; _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; }