![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 47:21bf4096faa1
- Parent:
- 46:b9081aa50bda
- Child:
- 48:b2afe48ced0d
diff -r b9081aa50bda -r 21bf4096faa1 main.cpp --- a/main.cpp Wed Mar 20 22:31:15 2019 +0000 +++ b/main.cpp Thu Mar 21 16:45:30 2019 +0000 @@ -1,12 +1,21 @@ /*TODO: Change: Indx - newCmd + _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 @@ -53,9 +62,9 @@ #define MCSNpin A0 // "Lacros" for utility -#define sgn(x) ((x)/abs(x)) -#define max(x,y) ((x)>=(y)?(x):(y)) -#define min(x,y) ((x)>=(y)?(y):(x)) +#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); @@ -81,8 +90,9 @@ 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 +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 + class Comm{ @@ -90,65 +100,57 @@ 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; // - volatile uint32_t _motorTorque; // Motor Toque - volatile uint64_t _newKey; // hash key - Mutex _newKeyMutex; // Restrict access to prevent deadlock. + 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 + Mutex _newKeyMutex; // Restrict access to prevent deadlock. - RawSerial _pc; - Thread _t_comm_out; - bool _RUN; + RawSerial _pc; + Thread _tCommOut; + bool _RUN; - enum msgType { motorState, posIn, velIn, posOut, velOut, + enum msgType { motorState, posIn, velIn, posOut, velOut, hashRate, keyAdded, nonceMatch, torque, rotations, melody, error}; - typedef struct { - msgType type; - uint32_t message; - } msg; + typedef struct { msgType type; + uint32_t message;} msg; - Mail<msg, 32> mailStack; - - //public: + Mail<msg, 32> _msgStack; + - //--------- Default Constructor With Inheritance From RawSerial Constructor ---------// - Comm(): _pc(SERIAL_TX, SERIAL_RX), _t_comm_out(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(18){ - - _pc.printf("\n\r%s\n\r", "Welcome" ); - // _MAXCMDLENGTH = 18; + + //------------- Default Constructor With Inheritance From RawSerial Constructor -------------// + Comm(): _pc(SERIAL_TX, SERIAL_RX), _tCommOut(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(18){ - _pc.putc('>'); - for (int i = 0; i < _MAXCMDLENGTH; ++i) { // reset buffer - inCharQ[i] = (char)'.'; // MbedOS prints 'Embedded Systems are fun and do awesome things!' - // _pc.putc('.'); // if you print a null terminator - } + _cmdIndex = 0; + _inCharIndex = 0; - + _outMining = false; + _motorTorque = 300; + _targetRot = 459.0; + _targetVel = 45.0; - inCharQ[_MAXCMDLENGTH] = (char)'\0'; - sprintf(inCharQ, "%s", inCharQ); // sorts out the correct string correctly - strncpy(newCmd, inCharQ, _MAXCMDLENGTH); - - _pc.printf("%s\n\r", inCharQ); - - _pc.putc('<'); //_pc.putc('\r'); _pc.putc('>'); + _modeBitField = 0x01; // Default velocity mode - _cmdIndex = 0; - - _inCharIndex = 0; - _outMining = false; - _pc.attach(callback(this, &Comm::serialISR)); + _pc.printf("\n\r%s\n\r", "Welcome\n>" ); // Welcome + //_pc.putc('>'); + 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!' - _motorTorque = 300; - _targetVel = 45.0; - _targetRot = 459.0; + _inCharQ[_MAXCMDLENGTH] = (char)'\0'; + sprintf(_inCharQ, "%s", _inCharQ); // Handling of the correct string + strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH); - _modeBitField = 0x01; // Default is velocity mode + _pc.printf("%s\n\r", _inCharQ); + _pc.putc('<'); + _pc.attach(callback(this, &Comm::serialISR)); } //--------- Interrupt Service Routine for Serial Port and Character Queue Handling ---------// @@ -157,25 +159,26 @@ char newChar = _pc.getc(); if (_inCharIndex == (_MAXCMDLENGTH)) { - inCharQ[_MAXCMDLENGTH] = '\0'; // force the string to have an end character + _inCharQ[_MAXCMDLENGTH] = '\0'; // Force the string to have an end character putMessage(error, 1); - _inCharIndex = 0; // reset buffer index + _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 + 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 - cmdParser(); - //parse the command for decoding. - for (int i = 0; i < _MAXCMDLENGTH; ++i) // reset buffer - inCharQ[i] = ' '; + _inCharQ[_inCharIndex] = '\0'; // When the command is finally over, + strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH); // Will copy 18 characters from _inCharQ to _newCmd - _inCharIndex = 0; // reset index + for (int i = 0; i < _MAXCMDLENGTH; ++i) // Reset buffer + _inCharQ[i] = ' '; + + _inCharIndex = 0; // Reset index + + cmdParser(); // Parse the command for decoding } } } @@ -185,231 +188,225 @@ void returnCursor() { _pc.putc('>'); for (int i = 0; i < _inCharIndex; ++i) - _pc.putc(inCharQ[i]); + _pc.putc(_inCharQ[i]); } - - //--------- Parse Incomming Data From Serial Port ---------// + + //--------- 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 + 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 + 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 '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 - if (miningTest == 1) - _outMining = true; - else - _outMining = false; + 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; - // This guy ugly, maybe use a function - case 'T': // Tune/ melody - uint8_t dur[9]; // Note Durations - char notes[9]; // Actual notes - uint8_t len = 0; // Length of notes - - 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] = '%'; - if (isChar) // if character - formatSpec[i+1] = 'c'; - else - 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, ¬es[0], &dur[0], - ¬es[1], &dur[1], - ¬es[2], &dur[2], - ¬es[3], &dur[3], - ¬es[4], &dur[4], - ¬es[5], &dur[5], - ¬es[6], &dur[6], - ¬es[7], &dur[7], - ¬es[8], &dur[8] - ); - _modeBitField = 0x08; - // putMessage(melody, newCmd); //Print it out - _pc.printf(formatSpec, notes[0], dur[0], \ - notes[1], dur[1], \ - notes[2], dur[2], \ - notes[3], dur[3], \ - notes[4], dur[4], \ - notes[5], dur[5], \ - notes[6], dur[6], \ - notes[7], dur[7], \ - notes[8], dur[8] \ - ); - } - else - putMessage(error, 2); // bad times - 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; - default: + 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; } } + 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 = mailStack.get(); + osEvent newEvent = _msgStack.get(); msg *pMessage = (msg *) newEvent.value.p; - //Case Switch to Choose Serial Output Based on Incoming Message Enum + //Case switch to choose serial output based on incoming message enum switch (pMessage->type) { case motorState: - _pc.printf("\r>%s< The motor is currently in state %x\n\r", inCharQ, pMessage->message); + _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); + _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); + _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); + _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); + _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); + _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)); + _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)); + _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 /*/ 6*/)); + _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: - _pc.printf("\r>%s< Debugging position:%x\n\r", inCharQ, pMessage->message); - for (int i = 0; i < _MAXCMDLENGTH; ++i) // reset buffer - inCharQ[i] = ' '; + 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); + _pc.printf("\r>%s< Unknown Error. Message: %x\n\r", _inCharQ, pMessage->message); break; } - mailStack.free(pMessage); + + + _msgStack.free(pMessage); } } void putMessage(msgType type, uint32_t message){ - msg *p_msg = mailStack.alloc(); + msg *p_msg = _msgStack.alloc(); p_msg->type = type; p_msg->message = message; - mailStack.put(p_msg); + _msgStack.put(p_msg); } void start_comm(){ _RUN = true; - - // for (int i = 0; i < _MAXCMDLENGTH; ++i) { // reset buffer - // inCharQ[i] = (char)'.'; // MbedOS prints 'Embedded Systems are fun and do awesome things!' - // } - - // inCharQ[_MAXCMDLENGTH] = (char)'\0'; - // sprintf(inCharQ, "%s", inCharQ); // sorts out the correct string correctly - // strncpy(newCmd, inCharQ, _MAXCMDLENGTH); - - _t_comm_out.start(callback(this, &Comm::commOutFn)); + _tCommOut.start(callback(this, &Comm::commOutFn)); } - char newCmd[]; // because unallocated must be defined at the bottom of the class - static char inCharQ[]; + char _newCmd[]; // Unallocated must be defined at the bottom of the class + static char _inCharQ[]; }; - -char Comm::inCharQ[] = {'.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','\0'}; + char Comm::_inCharQ[] = {'.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','\0'}; // Static member must be defined outside class class Motor { - protected: - int8_t orState; //Rotor offset at motor state 0, motor specific - volatile int8_t currentState; //Current Rotor State - volatile int8_t stateList[6]; //All possible rotor states stored - - //Phase lead to make motor spin - volatile int8_t lead; + 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 - Comm* p_comm; - bool _RUN; - - //Run the motor synchronisation + uint8_t theStates[3], // The Key states + stateCount[3]; // State Counter + uint32_t mtrPeriod, // Motor period + _MAXPWM_PRD; + float dutyC; // 1 = 100% + bool _RUN; - float dutyC; // 1 = 100% - uint32_t mtrPeriod; // motor period - uint8_t stateCount[3]; // State Counter - uint8_t theStates[3]; // The Key states + Comm* p_comm; + Thread t_motor_ctrl; // Thread for motor Control - Thread t_motor_ctrl; // Thread for motor Control - - uint32_t MAXPWM_PRD; public: - Motor() : t_motor_ctrl(osPriorityAboveNormal2, 1024) - { - // Set Power to maximum to drive motorHome() - dutyC = 1.0f; - mtrPeriod = 2e3; // motor period + Motor() : t_motor_ctrl(osPriorityAboveNormal2, 1024){ + + dutyC = 1.0f; // Set Power to maximum to drive motorHome() + mtrPeriod = 2e3; // Motor period pwmCtrl.period_us(mtrPeriod); pwmCtrl.pulsewidth_us(mtrPeriod); - orState = motorHome(); //Rotot offset at motor state 0 - currentState = readRotorState(); //Current Rotor State - // stateList[6] = {0,0,0, 0,0,0}; //All possible rotor states stored - lead = 2; //2 for forwards, -2 for backwards + orState = motorHome(); // Rotot offset at motor state 0 + currentState = readRotorState(); // 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; @@ -421,7 +418,7 @@ p_comm = NULL; // null pointer for now _RUN = false; - MAXPWM_PRD = 2e3; + _MAXPWM_PRD = 2e3; } @@ -523,30 +520,28 @@ uint8_t cpyCurrentState; int8_t cpyModeBitfield; - int32_t ting[2] = {6,1}; // 360,60 (for degrees), 5,1 (for states) + 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 - // static int32_t oldMotorPos = 0; //Old motor position used for calculations - // static uint8_t motorCtrlCounter = 0; //Counter to be reset every 10 iterations to get velocity calculation in seconds - volatile int32_t torque; //Local variable to set motor torque + 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 - float rError; //Rotation error between target and reality - static float rErrorOld; //Old rotation error used for calculation + 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 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 Ys; // Initialise controller output Ys (s=speed) + int32_t Yr; // Initialise controller output Yr (r=rotations) int32_t old_pos = 0; @@ -563,15 +558,14 @@ while (_RUN) { t_motor_ctrl.signal_wait((int32_t)0x1); - core_util_critical_section_enter(); - cpyModeBitfield = p_comm->_modeBitField; - // p_comm->_modeBitField = 0; // nah - //Access shared variables here - std::copy(stateCount, stateCount+3, cpyStateCount); - cpyCurrentState = currentState; - for (int i = 0; i < 3; ++i) { - stateCount[i] = 0; - } + 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; + for (int i = 0; i < 3; ++i) { + stateCount[i] = 0; + } core_util_critical_section_exit(); // read state & timestamp @@ -585,92 +579,58 @@ old_time = cur_time; 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; - if (stateDiff >= 0) { - totalDegrees = totalDegrees + (ting[1]* stateDiff); - } - - else { - totalDegrees = totalDegrees + (ting[1]*stateDiff*-1); - } - //p_comm->_pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10)); + stateDiff >= 0 ? totalDegrees = totalDegrees + (ting[1]* stateDiff) : \ + totalDegrees = totalDegrees + (ting[1]* stateDiff *-1); - if ((cpyModeBitfield & 0x01) | (cpyModeBitfield & 0x02)) { - //~~~~~Speed controller~~~~~~ + 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 = (p_comm->_targetVel * 6) - abs(cur_speed); // Read global variable _targetVel updated by interrupt and calculate error between target and reality - if (sError == -abs(cur_speed)) { //Check if user entered V0, - Ys = MAXPWM_PRD; //and set the output to maximum as specified - } - - else { - Ys = (int32_t)(Kp1 * sError); //If the user didn't enter V0 implement controller transfer function: Ys = Kp * (s -|v|) where, - } //Ys = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity + // Ys = Kp * (s -|v|) where, // SPEED CONTROLLER + // Ys = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity - // } else if (cpyModeBitfield & 0x02) { - //~~~~~Rotation control~~~~~~ - 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 Ys= Kp*Er + Kd* (dEr/dt) - rErrorOld = rError; //Update rotation error - // if(rError < 0) //Use the sign of the error to set controller wrt direction of rotation - // Ys = -Ys; + // 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: + + + // 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 + rErrorOld = rError; // Update rotation error + + Ys = (int32_t)( Ys * sgn(rError) ); // Use the sign of the error to set controller wrt direction of rotation - Ys = Ys * sgn(rError); - // select minimum absolute value torque - if (cur_speed < 0){ - torque = max(Ys, Yr); + cur_speed < 0 ? torque = max(Ys, Yr): torque = min(Ys, Yr); + + } + else if (cpyModeBitfield & 0x04) { // If it is in torque mode, do no PID math, just set pulsewidth + torque = (int32_t)p_comm->_motorTorque; + if (oldTorque != torque) { + p_comm->putMessage((Comm::msgType)8, torque); + oldTorque = torque; } - else{ - torque = min(Ys, Yr); - } + } + else{ + torque = _MAXPWM_PRD * 0.5; // Run at 50% duty cycle if argument not properly defined - if (torque < 0){ //Variable torque cannot be negative since it sets the PWM - torque = -torque; lead = -2; - } //Hence we make the value positive, - else{ //and instead set the direction to the opposite one - lead = 2; - } - - if(torque > MAXPWM_PRD){ //In case the calculated PWM is higher than our maximum 50% allowance, - torque = MAXPWM_PRD; //Set it to our max. - } - - p_comm->_motorTorque = torque; - pwmCtrl.pulsewidth_us(p_comm->_motorTorque); } - if (cpyModeBitfield & 0x04) { // if it is in torque mode, do no math, just set pulsewidth - torque = (int32_t)p_comm->_motorTorque; - if (oldTorque != torque) { - if(torque < 0){ //Variable torque cannot be negative since it sets the PWM - torque = -torque; //Hence we make the value positive, - lead = -2; //and instead set the direction to the opposite one - } else { - lead = 2; - } - if(torque > MAXPWM_PRD){ //In case the calculated PWM is higher than our maximum 50% allowance, - torque = MAXPWM_PRD; //Set it to our max. + torque < 0 ? lead = -2 : lead = +2; + torque = abs(torque); - } - p_comm->putMessage((Comm::msgType)8, torque); - p_comm->_motorTorque = torque; - pwmCtrl.pulsewidth_us(torque); - oldTorque = torque; - } - } - //else { // if not Torque mode - //balls - //} - // pwmCtrl.write((float)(p_comm->_motorTorque/MAXPWM_PRD)); - // p_comm->_motorTorque = torque; //Lastly, update global variable _motorTorque which is updated by interrupt - // p_comm->_pc.printf("\t\t\t\t\t\t %i, %i, %i \r", torque, Ys, Yr); - //p_comm->_pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10)); + 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; + pwmCtrl.pulsewidth_us(torque); + } } @@ -692,52 +652,58 @@ 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 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); - uint64_t* nonce = (uint64_t*)((int)sequence + 56); - uint8_t hash[32]; - uint32_t length64 = 64; - uint32_t hashCounter = 0; // Begin Main Timer Timer timer; timer.start(); - // Loop Program + // Loop Program while (1) { - // Mutex For Access Control - comm_port._newKeyMutex.lock(); - *key = comm_port._newKey; - comm_port._newKeyMutex.unlock(); + //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++; + // 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); - // Enum Casting and Condition - if ((hash[0]==0) && (hash[1]==0)){ - comm_port.putMessage((Comm::msgType)7, *nonce); - } + // Try Nonce + (*nonce)++; - // Try Nonce - (*nonce)++; + // Display via Comm Port + if (timer.read() >= 1){ + comm_port.putMessage((Comm::msgType)5, hashCounter); + hashCounter=0; + timer.reset(); + } + //} - // Display via Comm Port - if (timer.read() >= 1){ - comm_port.putMessage((Comm::msgType)5, hashCounter); - hashCounter=0; - timer.reset(); - } + //catch(...){ + // break; + //} + } return 0; - } \ No newline at end of file