Skad00sh
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
main.cpp
- Committer:
- CallumAlder
- Date:
- 2019-03-21
- Revision:
- 47:21bf4096faa1
- Parent:
- 46:b9081aa50bda
- Child:
- 48:b2afe48ced0d
File content as of revision 47:21bf4096faa1:
/*TODO:
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
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
/*
State L1 L2 L3
0 H - L
1 - H L
2 L H -
3 L - H
4 - L H
5 H L -
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 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);
//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};
//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
class Comm{
public:
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,
_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 _tCommOut;
bool _RUN;
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> _msgStack;
//------------- Default Constructor With Inheritance From RawSerial Constructor -------------//
Comm(): _pc(SERIAL_TX, SERIAL_RX), _tCommOut(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(18){
_cmdIndex = 0;
_inCharIndex = 0;
_outMining = false;
_motorTorque = 300;
_targetRot = 459.0;
_targetVel = 45.0;
_modeBitField = 0x01; // Default velocity mode
_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!'
_inCharQ[_MAXCMDLENGTH] = (char)'\0';
sprintf(_inCharQ, "%s", _inCharQ); // Handling of the correct string
strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH);
_pc.printf("%s\n\r", _inCharQ);
_pc.putc('<');
_pc.attach(callback(this, &Comm::serialISR));
}
//--------- 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{
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
for (int i = 0; i < _MAXCMDLENGTH; ++i) // Reset buffer
_inCharQ[i] = ' ';
_inCharIndex = 0; // Reset index
cmdParser(); // Parse the command for decoding
}
}
}
}
//--------- Reset Cursor Position ---------//
void returnCursor() {
_pc.putc('>');
for (int i = 0; i < _inCharIndex; ++i)
_pc.putc(_inCharQ[i]);
}
//--------- 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
_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
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
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 = _msgStack.get();
msg *pMessage = (msg *) newEvent.value.p;
//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));
break;
case melody:
_pc.printf("\r>%s< New Tune:\t%s\n\r", _inCharQ, _newCmd);
break;
case error:
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);
break;
}
_msgStack.free(pMessage);
}
}
void putMessage(msgType type, uint32_t message){
msg *p_msg = _msgStack.alloc();
p_msg->type = type;
p_msg->message = message;
_msgStack.put(p_msg);
}
void start_comm(){
_RUN = true;
_tCommOut.start(callback(this, &Comm::commOutFn));
}
char _newCmd[]; // Unallocated must be defined at the bottom of the class
static char _inCharQ[];
};
char Comm::_inCharQ[] = {'.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','\0'}; // Static member must be defined outside class
class Motor {
protected:
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
uint8_t theStates[3], // The Key states
stateCount[3]; // State Counter
uint32_t mtrPeriod, // Motor period
_MAXPWM_PRD;
float dutyC; // 1 = 100%
bool _RUN;
Comm* p_comm;
Thread t_motor_ctrl; // Thread for motor Control
public:
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
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));
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\r", 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;
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();
}
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]++;
// (Current - Offset + lead + 6) %6
motorOut((currentState - orState + lead + 6) % 6);
}
// 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;
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;
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
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;
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);
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
cur_time = m_timer.read();
// compute speed
time_diff = cur_time - old_time;
// cur_speed = (cur_pos - old_pos) / time_diff;
// prep values for next time through loop
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;
stateDiff >= 0 ? totalDegrees = totalDegrees + (ting[1]* stateDiff) : \
totalDegrees = totalDegrees + (ting[1]* stateDiff *-1);
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
// Ys = Kp * (s -|v|) where, // SPEED CONTROLLER
// Ys = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity
// 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
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 = _MAXPWM_PRD * 0.5; // Run at 50% duty cycle if argument not properly defined
}
torque < 0 ? lead = -2 : lead = +2;
torque = abs(torque);
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);
}
}
void motorCtrlTick(){
t_motor_ctrl.signal_set(0x1);
}
};
int main() {
// Declare Objects
Comm comm_port;
SHA256 miner;
Motor motor;
// 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,
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);
// Begin Main Timer
Timer timer;
timer.start();
// Loop Program
while (1) {
//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++;
// Enum Casting and Condition
if (hash[0]==0 && hash[1]==0)
comm_port.putMessage((Comm::msgType)7, *nonce);
// Try Nonce
(*nonce)++;
// Display via Comm Port
if (timer.read() >= 1){
comm_port.putMessage((Comm::msgType)5, hashCounter);
hashCounter=0;
timer.reset();
}
//}
//catch(...){
// break;
//}
}
return 0;
}