![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 44:990b5aaf5198
- Parent:
- 43:a6d20109b2f2
--- a/main.cpp Wed Mar 20 19:53:12 2019 +0000 +++ b/main.cpp Wed Mar 20 19:56:03 2019 +0000 @@ -1,14 +1,46 @@ +#include "SHA256.h" +#include "mbed.h" +// #include <iostream> +// #include "rtos.h" + /*TODO: Change: Indx newCmd - _MAXCMDLENGTH + 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 */ +//Photointerrupter input pins +#define I1pin D3 +#define I2pin D6 +#define I3pin D5 + +//Incremental encoder input pins +#define CHApin D12 +#define CHBpin D11 + +//Motor Drive output pins //Mask in output byte +#define L1Lpin D1 //0x01 +#define L1Hpin A3 //0x02 +#define L2Lpin D0 //0x04 +#define L2Hpin A6 //0x08 +#define L3Lpin D10 //0x10 +#define L3Hpin D2 //0x20 + +#define PWMpin D9 + +//Motor current sense +#define MCSPpin A1 +#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)) + //Mapping from sequential drive states to motor phase outputs /* State L1 L2 L3 @@ -22,59 +54,22 @@ 7 - - - */ -//Header Files -#include "SHA256.h" -#include "mbed.h" - -//Photointerrupter Input Pins -#define I1pin D3 -#define I2pin D6 -#define I3pin D5 - -//Incremental Encoder Input Pins -#define CHApin D12 -#define CHBpin D11 - -//Motor Drive High Pins //Mask in output byte -#define L1Hpin A3 //0x02 -#define L2Hpin A6 //0x08 -#define L3Hpin D2 //0x20 - -//Motor Drive Low Pins -#define L1Lpin D1 //0x01 -#define L2Lpin D0 //0x04 -#define L3Lpin D10 //0x10 - -//Motor Pulse Width Modulation (PWM) Pin -#define PWMpin D9 - -//Motor current sense -#define MCSPpin A1 -#define MCSNpin A0 - -// "Lacros" for utility -#define sgn(x) ((x)/abs(x)) -#define max(x,y) ((x)>=(y)?(x):(y)) -#define min(x,y) ((x)>=(y)?(y):(x)) - //Status LED DigitalOut led1(LED1); -//Photointerrupter Inputs +//Photointerrupter inputs InterruptIn I1(I1pin); InterruptIn I2(I2pin); InterruptIn I3(I3pin); -//Motor Drive High Outputs +//Motor Drive outputs +DigitalOut L1L(L1Lpin); DigitalOut L1H(L1Hpin); +DigitalOut L2L(L2Lpin); DigitalOut L2H(L2Hpin); +DigitalOut L3L(L3Lpin); DigitalOut L3H(L3Hpin); -//Motor Drive Low Outputs -DigitalOut L1L(L1Lpin); -DigitalOut L2L(L2Lpin); -DigitalOut L3L(L3Lpin); - PwmOut pwmCtrl(PWMpin); //Drive state to output table @@ -87,158 +82,135 @@ class Comm{ public: - - volatile bool _outMining; - volatile float _targetVel, _targetRot; + + Thread t_comm_out; + // Thread *p_motor_ctrl; + + bool _RUN; + + RawSerial pc; + // Queue<void, 8> inCharQ; // Input Character Queue + + + static const char MsgChar[11]; + + uint8_t MAXCMDLENGTH; + + volatile uint8_t cmdIndx; + volatile uint8_t inCharQIdx; + + volatile uint32_t motorPower; // motor toque + volatile float targetVel; + volatile float targetRot; + + volatile bool outMining; - 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. - - RawSerial _pc; - Thread _t_comm_out; - bool _RUN; - - enum msgType { motorState, posIn, velIn, posOut, velOut, - hashRate, keyAdded, nonceMatch, - torque, rotations, melody, - error}; - + enum msgType {motorState, posIn, velIn, posOut, velOut, + + hashRate, keyAdded, nonceMatch, + + torque, rotations, melody, + + error}; + typedef struct { msgType type; uint32_t message; } msg; - + Mail<msg, 32> mailStack; - - //public: - - //--------- 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; - - _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 - } - _pc.putc('<'); _pc.putc('\r'); _pc.putc('>'); - - inCharQ[_MAXCMDLENGTH] = (char)'\0'; - sprintf(inCharQ, "%s", inCharQ); // sorts out the correct string correctly - strncpy(newCmd, inCharQ, _MAXCMDLENGTH); - _cmdIndex = 0; - - _inCharIndex = 0; - _outMining = false; - _pc.attach(callback(this, &Comm::serialISR)); - - _motorTorque = 300; - _targetVel = 45.0; - _targetRot = 459.0; - - _modeBitField = 0x01; // Default is velocity mode - } - - //--------- Interrupt Service Routine for Serial Port and Character Queue Handling ---------// + int8_t modeBitfield; // 0,0,0,0,Melody,Torque,Rotation,Velocity + void serialISR(){ - if (_pc.readable()) { - char newChar = _pc.getc(); - - if (_inCharIndex == (_MAXCMDLENGTH)) { - inCharQ[_MAXCMDLENGTH] = '\0'; // force the string to have an end character + if (pc.readable()) { + char newChar = pc.getc(); + // inCharQ.put((void*)newChar); // void* = pointer to an unknown type that cannot be dereferenced + + if (inCharQIdx == (MAXCMDLENGTH)) { + inCharQ[MAXCMDLENGTH] = '\0'; // force the string to have an end character putMessage(error, 1); - _inCharIndex = 0; // reset buffer index + inCharQIdx = 0; // reset buffer index + // pc.putc('\r'); // carriage return moves to the start of the line + // for (int i = 0; i < MAXCMDLENGTH; ++i) + // { + // inCharQ[i] = ' '; + // pc.putc(' '); + // } + + // pc.putc('\r'); // carriage return moves to the start of the line } else{ - if(newChar != '\r'){ //While the command is not over, - inCharQ[_inCharIndex] = newChar; //save input character and - _inCharIndex++; //advance index - _pc.putc(newChar); + if(newChar != '\r'){ //While the command is not over, + inCharQ[inCharQIdx] = newChar; //save input character and + inCharQIdx++; //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[inCharQIdx] = '\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] = ' '; - - _inCharIndex = 0; // reset index + inCharQIdx = 0; // reset index } } } + + } - - //--------- Reset Cursor Position ---------// + void returnCursor() { - _pc.putc('>'); - for (int i = 0; i < _inCharIndex; ++i) - _pc.putc(inCharQ[i]); + pc.putc('>'); + for (int i = 0; i < inCharQIdx; ++i) // reset cursor position + pc.putc(inCharQ[i]); + // for (int i = inCharQIdx; i < MAXCMDLENGTH; ++i) // fill remaining with blanks + // pc.putc(' '); + // pc.putc('<'); } - - //--------- Parse Incomming Data From Serial Port ---------// + void cmdParser(){ switch(newCmd[0]) { - case 'K': //keyAdded - _newKeyMutex.lock(); //Ensure there is no deadlock - sscanf(newCmd, "K%x", &_newKey); //Find desired the Key code - putMessage(keyAdded, _newKey); //Print it out - _newKeyMutex.unlock(); + case 'K': // keyAdded + newKey_mutex.lock(); //Ensure there is no deadlock + sscanf(newCmd, "K%x", &newKey); //Find desired the Key code + putMessage(keyAdded, newKey); //Print it out + newKey_mutex.unlock(); + break; + case 'V': // velIn + sscanf(newCmd, "V%f", &targetVel); //Find desired the target velocity + modeBitfield = 0x01; + putMessage(velIn, targetVel); //Print it out 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 + case 'R': // posIn + sscanf(newCmd, "R%f", &targetRot); //Find desired target rotation + modeBitfield = 0x02; + putMessage(posIn, targetRot); //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 'x': // torque + sscanf(newCmd, "x%u", &motorPower); //Find desired target torque + modeBitfield = 0x04; + putMessage(torque, motorPower); //Print it out break; + case 'T': // Tune/ melody + uint8_t dur[9]; + char notes[9]; + uint8_t len = 0; - 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; - 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 # + // sscanf(newCmd, "T%s", &motorPower); //Find desired target torque + for (int i = 1; i < MAXCMDLENGTH; ++i) { if (newCmd[i] == '#') { len = i; - break; // stop at first # found + break; // stop at first } } - if (len>0) { // Parse the input only if # found + if (len>0) { 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 + for (int i = 1; i < newLen; i=i+2) { formatSpec[i] = '%'; if (isChar) // if character formatSpec[i+1] = 'c'; @@ -248,8 +220,8 @@ } formatSpec[newLen] = '\0'; - sprintf(formatSpec, "%s", formatSpec); // Set string format correctly - _pc.printf("%s\n", formatSpec ); + sprintf(formatSpec, "%s", formatSpec); + pc.printf("%s\n", formatSpec ); sscanf(newCmd, formatSpec, ¬es[0], &dur[0], ¬es[1], &dur[1], ¬es[2], &dur[2], @@ -260,9 +232,9 @@ ¬es[7], &dur[7], ¬es[8], &dur[8] ); - _modeBitField = 0x08; + modeBitfield = 0x08; // putMessage(melody, newCmd); //Print it out - _pc.printf(formatSpec, notes[0], dur[0], \ + pc.printf(formatSpec, notes[0], dur[0], \ notes[1], dur[1], \ notes[2], dur[2], \ notes[3], dur[3], \ @@ -274,92 +246,161 @@ ); } else - putMessage(error, 2); // bad times + putMessage(error, 2); break; break; - - default: + case 'M': // mining toggle + int8_t miningTest; + sscanf(newCmd, "M%d", &miningTest); //Find desired target torque + if (miningTest == 1) + outMining = true; + else + outMining = false; break; + default: break; } } - - //--------- Decode Messages to Print on Serial Port ---------// + + //~~~~~Decode messages to print on serial port~~~~~ void commOutFn() { while (_RUN) { osEvent newEvent = mailStack.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 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); + if (outMining) { + pc.printf("\r>%s< Mining: %.4u Hash/s\r", inCharQ, (uint32_t) pMessage->message); returnCursor(); - _outMining = false; + outMining = false; } break; case nonceMatch: - _pc.printf("\r>%s< Nonce found: %x\n\r", inCharQ, pMessage->message); + // if (outMining) { + 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 /*/ 6*/)); 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 /*/ 6*/)); 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 /*/ 6*/)); 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" ); + } + for (int i = 0; i < MAXCMDLENGTH-1; ++i) // reset buffer + inCharQ[i] = ' '; 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); } } + + + + + //TODO: stop function, maybe use parent de-constructor + //void stop_comm{} + + // public: + + volatile uint64_t newKey; // hash key + Mutex newKey_mutex; // Restrict access to prevent deadlock. + + Comm() : pc(SERIAL_TX, SERIAL_RX), + t_comm_out(osPriorityAboveNormal, 1024) + { // inherit from the RawSerial constructor + + pc.printf("\n\r%s\n\r", "Welcome" ); + MAXCMDLENGTH = 18; + + // reset buffer + // MbedOS prints 'Embedded Systems are fun and do awesome things!' + // if you print a null terminator + pc.putc('>'); + for (int i = 0; i < MAXCMDLENGTH; ++i) { + inCharQ[i] = (char)'.'; + pc.putc('.'); + } + pc.putc('<'); pc.putc('\r'); pc.putc('>'); + inCharQ[MAXCMDLENGTH] = (char)'\0'; + sprintf(inCharQ, "%s", inCharQ); // sorts out the correct string correctly + strncpy(newCmd, inCharQ, MAXCMDLENGTH); + + cmdIndx = 0; + + inCharQIdx = 0; + outMining = false; + pc.attach(callback(this, &Comm::serialISR)); + + motorPower = 300; + targetVel = 45.0; + targetRot = 459.0; + + modeBitfield = 0x01; // Default is velocity mode + + } + + void putMessage(msgType type, uint32_t message){ msg *p_msg = mailStack.alloc(); p_msg->type = type; p_msg->message = message; mailStack.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!' + + + // reset buffer + // MbedOS prints 'Embedded Systems are fun and do awesome things!' + // if you print a null terminator + for (int i = 0; i < MAXCMDLENGTH; ++i) { + inCharQ[i] = (char)'.'; } - - 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)); - + + 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)); + + + } - + char newCmd[]; // because unallocated must be defined at the bottom of the class char inCharQ[]; }; @@ -372,26 +413,26 @@ 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; - + Comm* p_comm; bool _RUN; - + //Run the motor synchronisation - + float dutyC; // 1 = 100% uint32_t mtrPeriod; // motor period uint8_t stateCount[3]; // State Counter uint8_t theStates[3]; // The Key states - + 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() @@ -399,29 +440,29 @@ 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 - + // 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; - + stateCount[0] = 0; stateCount[1] = 0; stateCount[2] = 0; - + p_comm = NULL; // null pointer for now _RUN = false; - + MAXPWM_PRD = 2e3; - + } - - + + void motorStart(Comm *comm) { - + // Establish Photointerrupter Service Routines (auto choose next state) I1.fall(callback(this, &Motor::stateUpdate)); I2.fall(callback(this, &Motor::stateUpdate)); @@ -429,31 +470,31 @@ 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 - + // Default a lower duty cylce dutyC = 0.8; pwmCtrl.period_us((uint32_t)mtrPeriod); pwmCtrl.pulsewidth_us((uint32_t)mtrPeriod*dutyC); - + p_comm = comm; _RUN = true; // Start motor control thread t_motor_ctrl.start(callback(this, &Motor::motorCtrlFn)); - p_comm->_pc.printf("origin=%i, theStates=[%i,%i,%i]\n\r", orState, theStates[0], theStates[1], theStates[2]); + p_comm->pc.printf("origin=%i, theStates=[%i,%i,%i]\n", orState, theStates[0], theStates[1], theStates[2]); } - + //Set a given drive state void motorOut(int8_t driveState) { - + //Lookup the output byte from the drive state. int8_t driveOut = driveTable[driveState & 0x07]; - + //Turn off first if (~driveOut & 0x01) L1L = 0; if (~driveOut & 0x02) L1H = 1; @@ -461,7 +502,7 @@ 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; @@ -470,51 +511,51 @@ if (driveOut & 0x10) L3L = 1; if (driveOut & 0x20) L3H = 0; } - + //Convert photointerrupter inputs to a rotor state inline int8_t readRotorState() { return stateMap[I1 + 2*I2 + 4*I3]; } - + //Basic synchronisation routine int8_t motorHome() { //Put the motor in drive state 0 and wait for it to stabilise motorOut(0); wait(3.0); - + //Get the rotor state return readRotorState(); } - - + + void stateUpdate() { // () { // **params currentState = readRotorState(); - + // Store into state counter if (currentState == theStates[0]) - stateCount[0]++; - else if (currentState == theStates[1]) - stateCount[1]++; - else if (currentState == theStates[2]) - stateCount[2]++; - - + stateCount[0]++; + else if (currentState == theStates[1]) + stateCount[1]++; + else if (currentState == theStates[2]) + stateCount[2]++; + + // (Current - Offset + lead + 6) %6 motorOut((currentState - orState + lead + 6) % 6); - + } - - - - // attach_us -> runs funtion every 100ms + + + + // attach_us -> runs funtion every 100ms 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; + uint8_t cpyStateCount[3]; + uint8_t cpyCurrentState; int8_t cpyModeBitfield; int32_t ting[2] = {6,1}; // 360,60 (for degrees), 5,1 (for states) @@ -533,12 +574,12 @@ 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; - - + float Kd=15.5; + + int32_t Ys; //Initialise controller output Ys (s=speed) int32_t Yr; //Initialise controller output Yr (r=rotations) @@ -558,13 +599,13 @@ t_motor_ctrl.signal_wait((int32_t)0x1); core_util_critical_section_enter(); - cpyModeBitfield = p_comm->_modeBitField; - // p_comm->_modeBitField = 0; // nah + cpyModeBitfield = p_comm->modeBitfield; + // p_comm->modeBitfield = 0; // nah //Access shared variables here - std::copy(stateCount, stateCount+3, cpyStateCount); + 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(); @@ -580,94 +621,85 @@ old_pos = cpyCurrentState; - iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount; + 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); + 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)); - + //p_comm->pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10)); + if ((cpyModeBitfield & 0x01) | (cpyModeBitfield & 0x02)) { //~~~~~Speed controller~~~~~~ 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 - - if (sError == -abs(cur_speed)) { //Check if user entered V0, + 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 { + } 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 - + // } 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) + 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; + // Ys = -Ys; Ys = Ys * sgn(rError); // select minimum absolute value torque - if (cur_speed < 0){ + if (cur_speed < 0) torque = max(Ys, Yr); - } - else{ + else torque = min(Ys, Yr); - } - 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 + 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); + p_comm->motorPower = torque; + pwmCtrl.pulsewidth_us(p_comm->motorPower); } - if (cpyModeBitfield & 0x04) { // if it is in torque mode, do no math, just set pulsewidth - torque = (int32_t)p_comm->_motorTorque; + if (cpyModeBitfield & 0x04) { // if it is in torque mode, do no math, just set pulsewidth + torque = (int32_t)p_comm->motorPower; if (oldTorque != torque) { - if(torque < 0){ //Variable torque cannot be negative since it sets the PWM - torque = -torque; //Hence we make the value positive, + 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. - + } p_comm->putMessage((Comm::msgType)8, torque); - p_comm->_motorTorque = torque; + p_comm->motorPower = 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)); + } else { // if not Torque mode + //balls + } + // pwmCtrl.write((float)(p_comm->motorPower/MAXPWM_PRD)); + // p_comm->motorPower = torque; //Lastly, update global variable motorPower 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)); } } - + void motorCtrlTick(){ t_motor_ctrl.signal_set(0x1); } @@ -680,7 +712,7 @@ Comm comm_port; SHA256 miner; Motor motor; - + // Start Motor and Comm Port motor.motorStart(&comm_port); comm_port.start_comm(); @@ -699,23 +731,23 @@ uint8_t hash[32]; uint32_t length64 = 64; uint32_t hashCounter = 0; - + // Begin Main Timer Timer timer; timer.start(); - + // Loop Program while (1) { - + // Mutex For Access Control - comm_port._newKeyMutex.lock(); - *key = comm_port._newKey; - comm_port._newKeyMutex.unlock(); - + comm_port.newKey_mutex.lock(); + *key = comm_port.newKey; + comm_port.newKey_mutex.unlock(); + // Compute Hash and Counter miner.computeHash(hash, sequence, length64); hashCounter++; - + // Enum Casting and Condition if ((hash[0]==0) && (hash[1]==0)){ comm_port.putMessage((Comm::msgType)7, *nonce); @@ -723,7 +755,7 @@ // Try Nonce (*nonce)++; - + // Display via Comm Port if (timer.read() >= 1){ comm_port.putMessage((Comm::msgType)5, hashCounter); @@ -731,7 +763,7 @@ timer.reset(); } } - + return 0; - + } \ No newline at end of file