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