Skad00sh
/
ES_CW2_Dev
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Crypto
Revision 47:21bf4096faa1, committed 2019-03-21
- Comitter:
- CallumAlder
- Date:
- Thu Mar 21 16:45:30 2019 +0000
- Parent:
- 46:b9081aa50bda
- Child:
- 48:b2afe48ced0d
- Commit message:
- midday food
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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