Skad00sh
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Revision 44:990b5aaf5198, committed 2019-03-20
- Comitter:
- CallumAlder
- Date:
- Wed Mar 20 19:56:03 2019 +0000
- Parent:
- 43:a6d20109b2f2
- Commit message:
- compare prints
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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