Skad00sh
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Revision 42:121148278dae, committed 2019-03-20
- Comitter:
- CallumAlder
- Date:
- Wed Mar 20 16:41:44 2019 +0000
- Parent:
- 27:ce05fed3c1ea
- Child:
- 43:a6d20109b2f2
- Commit message:
- Begin Tidying and Junk
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Mar 16 18:25:38 2019 +0000
+++ b/main.cpp Wed Mar 20 16:41:44 2019 +0000
@@ -1,38 +1,14 @@
-#include "SHA256.h"
-#include "mbed.h"
-// #include <iostream>
-// #include "rtos.h"
-
/*TODO:
-Change
-Indx
-newCmd
-MAXCMDLENGTH
+Change:
+ Indx
+ 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
*/
-//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
-
//Mapping from sequential drive states to motor phase outputs
/*
State L1 L2 L3
@@ -45,6 +21,62 @@
6 - - -
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
+InterruptIn I1(I1pin);
+InterruptIn I2(I2pin);
+InterruptIn I3(I3pin);
+
+//Motor Drive High Outputs
+DigitalOut L1H(L1Hpin);
+DigitalOut L2H(L2Hpin);
+DigitalOut L3H(L3Hpin);
+
+//Motor Drive Low Outputs
+DigitalOut L1L(L1Lpin);
+DigitalOut L2L(L2Lpin);
+DigitalOut L3L(L3Lpin);
+
+PwmOut pwmCtrl(PWMpin);
+
//Drive state to output table
const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
@@ -52,485 +84,564 @@
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
-//Phase lead to make motor spin
-const int8_t lead = 2; //2 for forwards, -2 for backwards
+class Comm{
+
+ public:
-//Status LED
-DigitalOut led1(LED1);
+ RawSerial pc;
+ Thread t_comm_out;
+ bool _RUN;
-//Photointerrupter inputs
-InterruptIn I1(I1pin);
-InterruptIn I2(I2pin);
-InterruptIn I3(I3pin);
-
-//Motor Drive outputs
-DigitalOut L1L(L1Lpin);
-DigitalOut L1H(L1Hpin);
-DigitalOut L2L(L2Lpin);
-DigitalOut L2H(L2Hpin);
-DigitalOut L3L(L3Lpin);
-DigitalOut L3H(L3Hpin);
-
-PwmOut pwmCtrl(PWMpin);
+ int8_t modeBitfield; // 0,0,0,... <=> Torque,Rotation,Velocity
+ volatile uint8_t inCharIndex, cmdIndx, MAXCMDLENGTH; //
+ volatile uint32_t motorPower; // Motor Toque
+ volatile uint64_t newKey; // hash key
+ Mutex newKey_mutex; // Restrict access to prevent deadlock.
+
+ volatile float targetVel, targetRot;
+ volatile bool outMining;
-uint8_t stateCount[3];
-uint8_t theStates[3];
+ static const char MsgChar[11];
-class Comm /*: public T_*/{
-
-public:
+ enum msgType { motorState, posIn, velIn, posOut, velOut,
+ hashRate, keyAdded, nonceMatch,
+ torque, rotations,
+ error};
- Thread t_comm_out;
- Thread t_motor_ctrl;
- // Thread *p_motor_ctrl;
-
- bool _RUN;
+ typedef struct {
+ msgType type;
+ uint32_t message;
+ } msg;
- RawSerial pc;
- // Queue<void, 8> inCharQ; // Input Character Queue
-
+ Mail<msg, 32> mailStack;
+
+ //public:
- static const char MsgChar[11];
-
- uint8_t MAXCMDLENGTH;
+ //--------- Default Constructor With Inheritance From RawSerial Constructor ---------//
+ Comm(): pc(SERIAL_TX, SERIAL_RX), t_comm_out(osPriorityAboveNormal, 1024){
- volatile uint8_t cmdIndx;
- volatile uint8_t inCharQIdx;
-
- volatile uint32_t motorPower; // motor toque
- volatile float targetVel;
- volatile float targetRot;
-
- enum msgType {motorState, posIn, velIn, posOut, velOut,
-
- hashRate, keyAdded, nonceMatch,
+ pc.printf("%s\n\r", "Welcome" );
+ MAXCMDLENGTH = 18;
- torque, rotations,
-
- error};
+ pc.putc('>');
+ for (int i = 0; i < MAXCMDLENGTH; ++i) { // reset buffer
+ inCharQ[i] = '.'; // MbedOS prints 'Embedded Systems are fun and do awesome things!'
+ pc.putc('.'); // if you print a null terminator
+ }
+ pc.putc('<');
+ pc.putc('\r');
- typedef struct {
- msgType type;
- uint32_t message;
- } msg;
+ inCharQ[MAXCMDLENGTH] = '\0';
+ strncpy(newCmd, inCharQ, MAXCMDLENGTH);
- Mail<msg, 32> mailStack;
+ cmdIndx = 0;
- void serialISR(){
- if (pc.readable()) {
- char newChar = pc.getc();
- // inCharQ.put((void*)newChar); // void* = pointer to an unknown type that cannot be dereferenced
+ inCharIndex = 0;
+ outMining = false;
+ pc.attach(callback(this, &Comm::serialISR));
+
+ motorPower = 300;
+ targetVel = 45.0;
+ targetRot = 459.0;
- if (inCharQIdx == (MAXCMDLENGTH)) {
- inCharQ[MAXCMDLENGTH] = '\0'; // force the string to have an end character
- putMessage(error, 1);
- 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(' ');
- // }
+ modeBitfield = 0x01; // Default is velocity mode
+ }
- // pc.putc('\r'); // carriage return moves to the start of the line
- }
- else{
- if(newChar != '\r'){ //While the command is not over,
- inCharQ[inCharQIdx] = newChar; //save input character and
- inCharQIdx++; //advance index
- pc.putc(newChar);
+ //--------- Interrupt Service Routine for Serial Port and Character Queue Handling ---------//
+ void serialISR(){
+ if (pc.readable()) {
+ char newChar = pc.getc();
+
+ if (inCharIndex == (MAXCMDLENGTH)) {
+ inCharQ[MAXCMDLENGTH] = '\0'; // force the string to have an end character
+ putMessage(error, 1);
+ inCharIndex = 0; // reset buffer index
}
else{
- 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] = ' ';
- inCharQIdx = 0; // reset 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] = ' ';
+
+ inCharIndex = 0; // reset index
+ }
}
}
}
-
- }
-
- /*void commInFn() {
- // if (_RUN)
+ //--------- Reset Cursor Position ---------//
+ void returnCursor() {
+ pc.putc('>');
+ for (int i = 0; i < inCharIndex; ++i)
+ pc.putc(inCharQ[i]);
+ }
- while (_RUN) {
- osEvent newEvent = inCharQ.get();
- uint8_t newChar = (uint8_t)(newEvent.value.p); // size_t to type cast the 64bit pointer properly
- pc.putc(newChar);
- if(cmdIndx >= MAXCMDLENGTH){ //Make sure there is no overflow in comand.
- cmdIndx = 0;
- putMessage(error, 1);
- }
- else{
- if(newChar != '\r'){ //While the command is not over,
- newCmd[cmdIndx] = newChar; //save input character and
- cmdIndx++; //advance index
- }
- else{
- newCmd[cmdIndx] = '\0'; //When the command is finally over,
- cmdIndx = 0; //reset index and
- cmdParser(); //parse the command for decoding.
- }
- }
- }
- }*/
+ //--------- Parse Incomming Data From Serial Port ---------//
+ void cmdParser(){
+ switch(newCmd[0]) {
+ case 'K': //(MsgChar[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;
- void returnCursor() {
- 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('<');
- }
+ case 'V': //(MsgChar[velIn])://
+ sscanf(newCmd, "V%f", &targetVel); //Find desired the target velocity
+ modeBitfield = 0x01; //Adjust bitfield pos 1
+ putMessage(velIn, targetVel); //Print it out
+ break;
- void cmdParser(){
- switch(newCmd[0]) {
- case 'K': //(MsgChar[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': //(MsgChar[velIn])://
- sscanf(newCmd, "V%f", &targetVel); //Find desired the target velocity
- putMessage(velIn, targetVel); //Print it out
- break;
- case 'R': //(MsgChar[posIn])://
- sscanf(newCmd, "R%f", &targetRot); //Find desired target rotation
- putMessage(posIn, targetRot); //Print it out
- break;
- case 'T': //(MsgChar[torque])://
- sscanf(newCmd, "T%d", &motorPower); //Find desired target torque
- putMessage(torque, motorPower); //Print it out
- break;
- default: break;
- }
- }
+ case 'R': //(MsgChar[posIn])://
+ sscanf(newCmd, "R%f", &targetRot); //Find desired target rotation
+ modeBitfield = 0x02; //Adjust bitfield pos 2
+ putMessage(posIn, targetRot); //Print it out
+ break;
- //~~~~~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
- switch (pMessage->type) {
- case motorState:
- pc.printf("The motor is currently in state %x\n\r", pMessage->message);
+ case 'T': //(MsgChar[torque])://
+ sscanf(newCmd, "T%u", &motorPower); //Find desired target torque
+ modeBitfield = 0x04; //Adjust bitfield pos 3
+ putMessage(torque, motorPower); //Print it out
break;
- case hashRate:
- pc.printf("\r>%s< Mining: %.4u Hash/s\r", inCharQ, (uint32_t) pMessage->message);
- returnCursor();
- break;
- case nonceMatch:
- pc.printf("\r>%s< Nonce found: %x\r", inCharQ, pMessage->message);
- returnCursor();
- break;
- case keyAdded:
- pc.printf("New Key Added:\t0x%016x\n\r", pMessage->message);
- break;
- case torque:
- pc.printf("Motor Torque set to:\t%d\n\r", pMessage->message);
- break;
- case velIn:
- pc.printf("Target Velocity set to:\t%.2f\n\r", targetVel);
+
+ case 'M': //(MsgChar[torque])://
+ int8_t miningTest;
+ sscanf(newCmd, "M%d", &miningTest); //Find desired target torque
+ if (miningTest == 1)
+ outMining = true;
+ else
+ outMining = false;
break;
- case velOut:
- pc.printf("Current Velocity:\t%.2f\n\r", \
- (float) ((int32_t) pMessage->message / 6));
- break;
- case posIn:
- pc.printf("Target Rotation set to:\t%.2f\n\r", \
- (float) ((int32_t) pMessage->message / 6));
- break;
- case posOut:
- pc.printf("Current Position:\t%.2f\n\r", \
- (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] = ' ';
- break;
- default:
- pc.printf("Unknown Error. Message: %x\n\r", pMessage->message);
+
+ default:
break;
}
- mailStack.free(pMessage);
- }
- }
-
- // attach_us -> runs funtion every 100ms
- void motorCtrlFn() {
- Ticker motorCtrlTicker;
- motorCtrlTicker.attach_us(callback(this,&Comm::motorCtrlTick), 1e5);
- uint8_t cpyStateCount[3];
- uint8_t cpyCurrentState;
-
- int16_t ting[2] = {5,1}; // 360,60 (for degrees), 5,1 (for states)
- uint8_t iterElementMax;
- int16_t totalDegrees;
- int16_t stateDiff;
-
- int32_t velocity; //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
- int32_t torque; //Local variable to set motor torque
- 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
-
- //~~~Controller constants~~~~
- int32_t Kp1=22; //Proportional controller constants
- int32_t Kp2=22; //Calculated by trial and error to give optimal accuracy
- float Kd=15.5;
-
-
- while (_RUN) {
- t_motor_ctrl.signal_wait((int32_t)0x1);
- core_util_critical_section_enter();
- //Access shared variables here
- std::copy(stateCount, stateCount+3, cpyStateCount);
- // TODO: A thing yes
- cpyCurrentState = 0;
- for (int i = 0; i < 3; ++i) {
- stateCount[i] = 0;
- }
- core_util_critical_section_exit();
-
- 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);
- }
- pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10));
-
- /*
- //~~~~~Speed controller~~~~~~
- velocity = totalDegrees*10;
- sError = (targetVel * 6) - abs(velocity); //Read global variable targetVel updated by interrupt and calculate error between target and reality
- int32_t Ys; //Initialise controller output Ys (s=speed)
- if (sError == -abs(velocity)) { //Check if user entered V0,
- Ys = MAXPWM; //and set the output to maximum as specified
- } else {
- Ys = (int)(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
-
- //~~~~~Rotation control~~~~~~
- rError = targetRot - (locMotorPos/6); //Read global variable targetRot updated by interrupt and calculate the rotation error.
- int32_t Yr; //Initialise controller output Yr (r=rotations)
- 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;
- }
-
- if((velocity>=0 && Ys<Yr) || (velocity<0 && Ys>Yr)){ //Choose Ys or Yr based on distance from target value so that it takes
- torque = Ys; //appropriate steps in the right direction to reach target value
- } else {
- torque = Yr;
- }
- 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){ //In case the calculated PWM is higher than our maximum 50% allowance,
- torque = MAXPWM; //Set it to our max.
- }
-
- motorPower = torque; //Lastly, update global variable motorPower which is updated by interrupt
- */
}
- }
+ //--------- Decode Messages to Print on Serial Port ---------//
+ void commOutFn() {
+ while (_RUN) {
+ osEvent newEvent = mailStack.get();
+ msg *pMessage = (msg *) newEvent.value.p;
- void motorCtrlTick(){
- t_motor_ctrl.signal_set(0x1);
- }
+ //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);
+ break;
+ case hashRate:
+ if (outMining) {
+ 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);
+ returnCursor();
+ break;
+ case keyAdded:
+ 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);
+ break;
+ case velIn:
+ 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));
+ break;
+ case posIn:
+ 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*/));
+ 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] = ' ';
+ break;
+ default:
+ pc.printf("\r>%s< Unknown Error. Message: %x\n\r", inCharQ, pMessage->message);
+ break;
+ }
+
+ mailStack.free(pMessage);
+ }
+ }
+
+ 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;
- //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),
- t_motor_ctrl(osPriorityAboveNormal2, 1024)
- { // inherit from the RawSerial constructor
-
- pc.printf("%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] = '.';
+ pc.putc('.');
+ }
+ pc.putc('<');
+ pc.putc('\r');
- // 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] = '.';
- pc.putc('.');
- }
- pc.putc('<');
- pc.putc('\r');
+ inCharQ[MAXCMDLENGTH] = '\0';
+ strncpy(newCmd, inCharQ, MAXCMDLENGTH);
+
+ // returnCursor();
- inCharQ[MAXCMDLENGTH] = '\0';
- strncpy(newCmd, inCharQ, MAXCMDLENGTH);
-
- cmdIndx = 0;
-
- inCharQIdx = 0;
- // inCharQIdx = MAXCMDLENGTH-1;
+ // t_comm_in.start(callback(this, &Comm::commInFn));
+ // this::thread::wait()
+ // wait(1.0);
+ t_comm_out.start(callback(this, &Comm::commOutFn));
- pc.attach(callback(this, &Comm::serialISR));
+ }
- // Thread t_comm_in(osPriorityAboveNormal, 1024);
- // Thread t_comm_out(osPriorityAboveNormal, 1024);
- // Thread t_motor_ctrl(osPriorityAboveNormal, 1024);
+ char newCmd[]; // because unallocated must be defined at the bottom of the class
+ char inCharQ[];
+};
- motorPower = 300;
- targetVel = 45.0;
- targetRot = 459.0;
+
+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
- /*MsgChar = {'m', 'R', 'V', 'r', 'v',
+ //Phase lead to make motor spin
+ volatile int8_t lead;
+
+ Comm* p_comm;
+ bool _RUN;
+
+ //Run the motor synchronisation
- 'h', 'K', 'n',
+ 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:
- 'T', 'r',
+ Motor() : t_motor_ctrl(osPriorityAboveNormal2, 1024)
+ {
+ // Set Power to maximum to drive motorHome()
+ dutyC = 1.0f;
+ 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
- 'e'};*/
- }
+ // 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 putMessage(msgType type, uint32_t message){
- msg *p_msg = mailStack.alloc();
- p_msg->type = type;
- p_msg->message = message;
- mailStack.put(p_msg);
- }
+ void motorStart(Comm *comm) {
+
+ // Establish Photointerrupter Service Routines (auto choose next state)
+ I1.fall(callback(this, &Motor::stateUpdate));
+ 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
+
+ // 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", orState, theStates[0], theStates[1], theStates[2]);
+
+ }
+
+ //Set a given drive state
+ void motorOut(int8_t driveState) {
- void start_comm(){
- _RUN = true;
+ //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;
+ 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;
+ }
+
+ //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();
+ }
- // 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] = '.';
- pc.putc('.');
- }
- pc.putc('<');
- pc.putc('\r');
+ void stateUpdate() { // () { // **params
+ currentState = readRotorState();
- inCharQ[MAXCMDLENGTH] = '\0';
- strncpy(newCmd, inCharQ, MAXCMDLENGTH);
-
- // returnCursor();
-
- // t_comm_in.start(callback(this, &Comm::commInFn));
- // this::thread::wait()
- // wait(1.0);
- t_comm_out.start(callback(this, &Comm::commOutFn));
- t_motor_ctrl.start(callback(this, &Comm::motorCtrlFn));
+ // Store into state counter
+ if (currentState == theStates[0])
+ 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);
- char newCmd[]; // because unallocated must be defined at the bottom of the class
- char inCharQ[];
-};
+ }
-//Set a given drive state
-void motorOut(int8_t driveState){
+ // attach_us -> runs funtion every 100ms
+ void motorCtrlFn() {
+ Ticker motorCtrlTicker;
+ Timer m_timer;
+ motorCtrlTicker.attach_us(callback(this,&Motor::motorCtrlTick), 1e5);
- //Lookup the output byte from the drive state.
- int8_t driveOut = driveTable[driveState & 0x07];
+ // Init some things
+ 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;
- //Turn off first
- if (~driveOut & 0x01) L1L = 0;
- 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;
+ 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
+ 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
+
+ //~~~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 Ys; //Initialise controller output Ys (s=speed)
+ int32_t Yr; //Initialise controller output Yr (r=rotations)
+
+ int32_t old_pos = 0;
- //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;
-}
+ uint32_t cur_time = 0,
+ old_time = 0,
+ time_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);
-//Convert photointerrupter inputs to a rotor state
-inline int8_t readRotorState(){
- return stateMap[I1 + 2*I2 + 4*I3];
-}
+ 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_exit();
-//Basic synchronisation routine
-int8_t motorHome() {
- //Put the motor in drive state 0 and wait for it to stabilise
- motorOut(0);
- wait(2.0);
+ // read state & timestamp
+ cur_time = m_timer.read();
+
+ // compute speed
+ time_diff = cur_time - old_time;
+ // cur_speed = (cur_pos - old_pos) / time_diff;
- //Get the rotor state
- return readRotorState();
-}
+ // prep values for next time through loop
+ old_time = cur_time;
+ old_pos = cpyCurrentState;
+
+
+ iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount;
-void stateUpdate(int8_t *params[]) { // () { // **params
- *params[0] = readRotorState();
- int8_t currentState = *params[0];
- int8_t offset = *params[1];
+ 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));
+
+ 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,
+ 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
+
+ // } 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;
+
+ Ys = Ys * sgn(rError);
+ // select minimum absolute value torque
+ if (cur_speed < 0){
+ torque = max(Ys, Yr);
+ }
+ else{
+ torque = min(Ys, Yr);
+ }
- switch (currentState) {
- case 1:
- stateCount[0]++;
- break;
- case (1 + lead):
- stateCount[1]++;
- break;
- case (1 + (lead*2)):
- stateCount[2]++;
- break;
- }
+ 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->motorPower = torque;
+ pwmCtrl.pulsewidth_us(p_comm->motorPower);
+ }
- motorOut((currentState - offset + lead + 6) % 6);
-}
+ 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,
+ 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.
-//Main
+ }
+ p_comm->putMessage((Comm::msgType)8, torque);
+ p_comm->motorPower = torque;
+ pwmCtrl.pulsewidth_us(torque);
+ oldTorque = torque;
+ }
+ }
+ //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);
+ }
+};
+
+
int main() {
- // std::ios::sync_with_stdio(false);
- Comm comm_plz;
+ // Declare Objects
+ Comm comm_port;
+ SHA256 miner;
+ Motor motor;
- // comm_plz.pc.printf("%s\n", "do i work bruh" ); // using printf of class is calm
- SHA256 Miner;
+ // Start Motor and Comm Port
+ motor.motorStart(&comm_port);
+ 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,
@@ -544,95 +655,39 @@
uint8_t hash[32];
uint32_t length64 = 64;
uint32_t hashCounter = 0;
- Timer timer;
-
- float dutyC = 1; // 100%
- float mtrPeriod = 2e-3; // motor period
-
- pwmCtrl.period(mtrPeriod);
- pwmCtrl.pulsewidth(mtrPeriod*dutyC);
-
- comm_plz.start_comm();
- // Motor States
- int8_t orState = 0; //Rotot offset at motor state 0
- int8_t currentState = 0; //Rotot offset at motor state 0
- int8_t stateList[6]; //Rotot offset at motor state 0
- //Run the motor synchronisation
- orState = motorHome();
-
- theStates[0] = orState;
- theStates[1] = (orState + lead) % 6;
- theStates[2] = (orState + (lead*2)) % 6;
+ // Begin Main Timer
+ Timer timer;
+ timer.start();
- // Add callbacks
- // I1.fall(&stateUpdate);
- // I2.fall(&stateUpdate);
- // I3.fall(&stateUpdate);
- int8_t* params[2];
- params[0] = ¤tState;
- params[1] = &orState;
-
- I1.fall(callback(&stateUpdate,params));
- I2.fall(callback(&stateUpdate,params));
- I3.fall(callback(&stateUpdate,params));
+ // Loop Program
+ while (1) {
- I1.rise(callback(&stateUpdate,params));
- I2.rise(callback(&stateUpdate,params));
- I3.rise(callback(&stateUpdate,params));
-
- // Push motor to move
- currentState = readRotorState();
- motorOut((currentState-orState+lead+6)%6); // We push it digitally
-
- // pc.printf("Rotor origin: %x\n\r",orState);
- // orState is subtracted from future rotor state inputs to align rotor and motor states
- // intState = readRotorState();
- //if (intState != intStateOld) {
- // pc.printf("old:%d \t new:%d \t next:%d \n\r",intStateOld, intState, (intState-orState+lead+6)%6);
- // intStateOld = intState;
- // motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
- // }
+ // Mutex For Access Control
+ comm_port.newKey_mutex.lock();
+ *key = comm_port.newKey;
+ comm_port.newKey_mutex.unlock();
- dutyC = 0.8;
- pwmCtrl.pulsewidth(mtrPeriod*dutyC);
-
+ // Compute Hash and Counter
+ miner.computeHash(hash, sequence, length64);
+ hashCounter++;
- // Keep the program running indefinitely
- timer.start(); // start timer
- int stateCount = 0;
- while (1) {
- // pc.printf("Current:%d \t Next:%d \n\r", currentState, (currentState-orState+lead+6)%6);
- comm_plz.newKey_mutex.lock();
- *key = comm_plz.newKey;
- comm_plz.newKey_mutex.unlock();
- Miner.computeHash(hash, sequence, length64);
- hashCounter++;
+ // Enum Casting and Condition
if ((hash[0]==0) && (hash[1]==0)){
- comm_plz.putMessage((Comm::msgType)7, *nonce);
+ comm_port.putMessage((Comm::msgType)7, *nonce);
}
- // Try a new nonce
+ // Try Nonce
(*nonce)++;
- if (stateCount<6){
- stateList[stateCount] = currentState;
- stateCount++;
- }
- else {
- //pc.printf("states");
- //for(int i = 0; i < 6; ++i)
- //pc.printf("%02i,", stateList[i]);
- //pc.printf("\n\r");
- stateCount = 0;
- }
-
- // Per Second i.e. when greater or equal to 1
+ // Display via Comm Port
if (timer.read() >= 1){
- comm_plz.putMessage((Comm::msgType)5, hashCounter);
- //pc.printf("HashRate = %02u \n\r",hashCounter);
+ comm_port.putMessage((Comm::msgType)5, hashCounter);
hashCounter=0;
timer.reset();
}
}
+
+ return 0;
+
}
\ No newline at end of file