![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 48:b2afe48ced0d
- Parent:
- 47:21bf4096faa1
- Child:
- 49:ae8dedfe2d0f
--- a/main.cpp Thu Mar 21 16:45:30 2019 +0000 +++ b/main.cpp Thu Mar 21 23:54:18 2019 +0000 @@ -93,7 +93,8 @@ 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: @@ -108,13 +109,13 @@ _noteDur[9],_noteLen; // Array of note durations volatile uint32_t _motorTorque; // Motor Toque volatile uint64_t _newKey; // hash key - Mutex _newKeyMutex; // Restrict access to prevent deadlock. + RawSerial _pc; Thread _tCommOut; bool _RUN; - enum msgType { motorState, posIn, velIn, posOut, velOut, + enum msgType { mot_orState, posIn, velIn, posOut, velOut, hashRate, keyAdded, nonceMatch, torque, rotations, melody, error}; @@ -125,9 +126,8 @@ Mail<msg, 32> _msgStack; - - //------------- Default Constructor With Inheritance From RawSerial Constructor -------------// - Comm(): _pc(SERIAL_TX, SERIAL_RX), _tCommOut(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(18){ + //-- Default Constructor With Inheritance From RawSerial Constructor ------------------------------------------------// + Comm(): _pc(SERIAL_TX, SERIAL_RX), _tCommOut(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(18){ _cmdIndex = 0; _inCharIndex = 0; @@ -139,13 +139,12 @@ _modeBitField = 0x01; // Default velocity mode - _pc.printf("\n\r%s\n\r", "Welcome\n>" ); // Welcome - //_pc.putc('>'); + _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[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 + sprintf(_inCharQ, "%s", _inCharQ); // Handling of the correct string strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH); _pc.printf("%s\n\r", _inCharQ); @@ -153,8 +152,8 @@ _pc.attach(callback(this, &Comm::serialISR)); } - //--------- Interrupt Service Routine for Serial Port and Character Queue Handling ---------// - void serialISR(){ + //-- Interrupt Service Routine for Serial Port and Character Queue Handling -----------------------------------------// + void serialISR() { if (_pc.readable()) { char newChar = _pc.getc(); @@ -177,22 +176,22 @@ _inCharQ[i] = ' '; _inCharIndex = 0; // Reset index - - cmdParser(); // Parse the command for decoding + YES = true; + // cmdParser(); // Parse the command for decoding } } } } - //--------- Reset Cursor Position ---------// + //-- 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(){ + //-- Parse Incoming Data From Serial Port ---------------------------------------------------------------------------// + void cmdParser() { switch(_newCmd[0]) { case 'K': // keyAdded _newKeyMutex.lock(); // Ensure there is no deadlock @@ -234,6 +233,7 @@ } } + //-- Read In Note Data From Serial Port and Parse Into Class Variables ----------------------------------------------// bool regexTune() { uint8_t len = 0; @@ -271,7 +271,7 @@ // Update _newCmd for putMessage print - sprintf(_newCmd,formatSpec, _notes[0], _noteDur[0],\ + sprintf(_newCmd,formatSpec, _notes[0], _noteDur[0],\ _notes[1], _noteDur[1],\ _notes[2], _noteDur[2],\ _notes[3], _noteDur[3],\ @@ -289,15 +289,19 @@ } } - //--------- Decode Messages to Print on Serial Port ---------// + //-- 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 motorState: + case mot_orState: _pc.printf("\r>%s< The motor is currently in state %x\n\r", _inCharQ, pMessage->message); break; case hashRate: @@ -355,9 +359,12 @@ _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; @@ -365,218 +372,204 @@ _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 + 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 +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 + 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 + uint8_t _theStates[3], // The Key states + _stateCount[3]; // State Counter uint32_t mtrPeriod, // Motor period _MAXPWM_PRD; - float dutyC; // 1 = 100% + float _dutyC; // 1 = 100% bool _RUN; - Comm* p_comm; - Thread t_motor_ctrl; // Thread for motor Control - + Comm* _pComm; + Thread _tMotorCtrl; // Thread for motor Control public: - - Motor() : t_motor_ctrl(osPriorityAboveNormal2, 1024){ + //-- 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); + _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(); // Rotot offset at motor state 0 - currentState = readRotorState(); // Current Rotor State - lead = 2; // 2 for forwards, -2 for backwards + _orState = motorHome(); // Rotor offset at motor state 0 + _currentState = readRot_orState(); // Current Rotor State + _lead = 2; // 2 for forwards, -2 for backwards - // It skips the origin state and it's 'lead' increments? - theStates[0] = orState +1; - theStates[1] = (orState + lead) % 6 +1; - theStates[2] = (orState + (lead*2)) % 6 +1; + _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; stateCount[1] = 0; stateCount[2] = 0; + _stateCount[0] = 0; // Initialise + _stateCount[1] = 0; + _stateCount[2] = 0; - p_comm = NULL; // null pointer for now - _RUN = false; + _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) { - // Establish Photointerrupter Service Routines (auto choose next state) - I1.fall(callback(this, &Motor::stateUpdate)); + 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)); - // push digitally so if motor is static it will start moving - motorOut((currentState-orState+lead+6)%6); // We push it digitally + motorOut((_currentState-_orState+_lead+6)%6); // Push digitally so static motor will start moving - // Default a lower duty cylce - dutyC = 0.8; + + _dutyC = 0.8; // Default a lower duty cycle pwmCtrl.period_us((uint32_t)mtrPeriod); - pwmCtrl.pulsewidth_us((uint32_t)mtrPeriod*dutyC); + pwmCtrl.pulsewidth_us((uint32_t)mtrPeriod*_dutyC); - p_comm = comm; + _pComm = comm; _RUN = true; - // Start motor control thread - t_motor_ctrl.start(callback(this, &Motor::motorCtrlFn)); + _tMotorCtrl.start(callback(this, &Motor::motorCtrlFn)); // Start motor control thread - p_comm->_pc.printf("origin=%i, theStates=[%i,%i,%i]\n\r", orState, theStates[0], theStates[1], theStates[2]); + _pComm->_pc.printf("origin=%i, _theStates=[%i,%i,%i]\n\r", _orState, _theStates[0], _theStates[1], _theStates[2]); // Print information to terminal } - //Set a given drive state + //-- Set a Predetermined Drive State -------------------------------------------------------------------------------// void motorOut(int8_t driveState) { - //Lookup the output byte from the drive state. - int8_t driveOut = driveTable[driveState & 0x07]; + int8_t driveOut = driveTable[driveState & 0x07]; //Lookup the output byte from the drive state - //Turn off first - if (~driveOut & 0x01) L1L = 0; + 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; - //Then turn on - if (driveOut & 0x01) L1L = 1; - if (driveOut & 0x02) L1H = 0; - if (driveOut & 0x04) L2L = 1; - if (driveOut & 0x08) L2H = 0; - if (driveOut & 0x10) L3L = 1; - if (driveOut & 0x20) L3H = 0; + 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; } - //Convert photointerrupter inputs to a rotor state - inline int8_t readRotorState() { + //-- Inline Conversion of Photointerrupts to a Rotor State ----------------------------------------------------------// + inline int8_t readRot_orState() { return stateMap[I1 + 2*I2 + 4*I3]; } - //Basic synchronisation routine + //-- Basic Motor Stabilisation and Synchronisation ------------------------------------------------------------------// int8_t motorHome() { - //Put the motor in drive state 0 and wait for it to stabilise - motorOut(0); + + motorOut(0); //Put the motor in drive state 0 and wait for it to stabilise wait(3.0); - //Get the rotor state - return readRotorState(); + return readRot_orState(); //Get the rotor state } - + //-- Motor State Log, Circumvents Issues From Occasionally Skipping Certain States ----------------------------------// void stateUpdate() { // () { // **params - currentState = readRotorState(); + _currentState = readRot_orState(); // Get current state - // Store into state counter - if (currentState == theStates[0]) - stateCount[0]++; - else if (currentState == theStates[1]) - stateCount[1]++; - else if (currentState == theStates[2]) - stateCount[2]++; + 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]++; - - // (Current - Offset + lead + 6) %6 - motorOut((currentState - orState + lead + 6) % 6); + motorOut((_currentState - _orState + _lead + 6) % 6); // Send the next state to the motor } - - - // attach_us -> runs funtion every 100ms + //-- 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; + 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 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 + 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 + 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 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 Ys; // Initialise controller output Ys (s=speed) - int32_t Yr; // Initialise controller output Yr (r=rotations) + int32_t Ts; // Initialise controller output Ts (s=speed) + int32_t Tr; // Initialise controller output Tr (r=rotations) - int32_t old_pos = 0; + int32_t old_pos = 0; - uint32_t cur_time = 0, - old_time = 0, - time_diff; + uint32_t cur_time = 0, + old_time = 0, + time_diff; - float cur_err = 0.0f, - old_err = 0.0f, - err_diff; + float cur_err = 0.0f, + old_err = 0.0f, + err_diff; m_timer.start(); while (_RUN) { - t_motor_ctrl.signal_wait((int32_t)0x1); + _tMotorCtrl.signal_wait((int32_t)0x1); - core_util_critical_section_enter(); //Access shared variables here - cpyModeBitfield = p_comm->_modeBitField; - // p_comm->_modeBitField = 0; // nah - std::copy(stateCount, stateCount+3, cpyStateCount); - cpyCurrentState = currentState; + 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; + _stateCount[i] = 0; } core_util_critical_section_exit(); - // read state & timestamp - cur_time = m_timer.read(); + + cur_time = m_timer.read(); // Read state & timestamp - // 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_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 @@ -584,37 +577,37 @@ iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount; totalDegrees = ting[0] * cpyStateCount[iterElementMax]; - stateDiff = theStates[iterElementMax]-cpyCurrentState; + 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 = (p_comm->_targetVel * 6) - abs(cur_speed); // Read global variable _targetVel updated by interrupt and calculate error between target and reality + sError = (_pComm->_targetVel * 6) - abs(cur_speed); // Read global variable _targetVel updated by interrupt and calculate error between target and reality - // Ys = Kp * (s -|v|) where, // SPEED CONTROLLER - // Ys = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity + // 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) ? Ys = _MAXPWM_PRD : \ - Ys = (Kp1 * sError); // If the user didn't enter V0 implement controller transfer function: + sError == -abs(cur_speed) ? Ts = _MAXPWM_PRD : \ + Ts = (Kp1 * sError); // If the user didn't enter V0 implement controller transfer function: - // Yr= Kp*Er + Kd* (dEr/dt) where, // ROTATION CONTROLLER - // Yr = controller output, Kp = prop controller constant, Er = error in number of rotations - rError = (p_comm->_targetRot)*6 - totalDegrees; // Read global variable _targetRot updated by interrupt and calculate the rotation error. - Yr = Kp2*rError + Kd*(rError - rErrorOld); // 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 - Ys = (int32_t)( Ys * sgn(rError) ); // Use the sign of the error to set controller wrt direction of rotation + Ts = (int32_t)( Ts * sgn(rError) ); // Use the sign of the error to set controller wrt direction of rotation - cur_speed < 0 ? torque = max(Ys, Yr): torque = min(Ys, Yr); + 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)p_comm->_motorTorque; + torque = (int32_t)_pComm->_motorTorque; if (oldTorque != torque) { - p_comm->putMessage((Comm::msgType)8, torque); + _pComm->putMessage((Comm::msgType)8, torque); oldTorque = torque; } } @@ -623,19 +616,19 @@ } - torque < 0 ? lead = -2 : lead = +2; + 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. - p_comm->_motorTorque = torque; + _pComm->_motorTorque = torque; pwmCtrl.pulsewidth_us(torque); } } void motorCtrlTick(){ - t_motor_ctrl.signal_set(0x1); + _tMotorCtrl.signal_set(0x1); } }; @@ -676,9 +669,9 @@ //try{ // Mutex For Access Control - comm_port._newKeyMutex.lock(); + _newKeyMutex.lock(); *key = comm_port._newKey; - comm_port._newKeyMutex.unlock(); + _newKeyMutex.unlock(); // Compute Hash and Counter miner.computeHash(hash, sequence, length64);