Skad00sh
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
main.cpp
- Committer:
- adehadd
- Date:
- 2019-05-07
- Revision:
- 56:0e7f794d2676
- Parent:
- 55:e8b15b4b875f
File content as of revision 56:0e7f794d2676:
//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);
//Encoder inputs
InterruptIn CHA(CHApin);
InterruptIn CHB(CHBpin);
//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
#ifndef MAXCMDLENGTH
#define MAXCMDLENGTH 18
#endif
#ifndef MAXCMDLENGTH_HALF
#define MAXCMDLENGTH_HALF 9
#endif
class Comm{
public:
volatile bool _outMining;
volatile float _targetVel, _targetRot;
volatile char _notes[MAXCMDLENGTH_HALF]; // Array of actual _notes
volatile int8_t _modeBitField; // 0,0,0,... <=> Melody,Torque,Rotation,Velocity
const uint8_t _MAXCMDLENGTH; //
volatile uint8_t _inCharIndex, _cmdIndex, _noteRep,
_noteDur[MAXCMDLENGTH_HALF],_noteLen; // Array of note durations
volatile uint32_t _motorTorque; // Motor Toque
volatile uint64_t _newKey; // hash key
volatile int32_t _motor_pos;
char _inCharQ[MAXCMDLENGTH],
_newCmd[MAXCMDLENGTH];
RawSerial _pc;
Thread _tCommOut, _tCommIn;
Mutex _newKeyMutex; // Restrict access to prevent deadlock.
bool _RUN;
enum msgType { mot_orState, posIn, velIn, posOut, velOut,
hashRate, keyAdded, nonceMatch,
torque, rotations, melody,
error};
typedef struct { msgType type;
uint32_t message;} msg; // TODO: Maybe add a thing that stores the newCmd message as well
Mail<msg, 32> _msgStack;
Mail<bool,32> _msgReceived;
//-- Default Constructor With Inheritance From RawSerial Constructor ------------------------------------------------//
Comm(): _pc(SERIAL_TX, SERIAL_RX), _tCommOut(osPriorityNormal, 1024), _tCommIn(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(MAXCMDLENGTH){
_cmdIndex = 0;
_inCharIndex = 0;
_outMining = false;
_motorTorque = 300;
_targetRot = 459.0;
_targetVel = 45.0;
_motor_pos = 0;
_modeBitField = 0x01; // Default velocity mode
_pc.printf("\n\r%s %d\n\r>", "Welcome", _MAXCMDLENGTH ); // Welcome
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);
}
void commInFn() {
_pc.attach(callback(this, &Comm::serialISR));
char newChar;
while (_RUN) {
osEvent newEvent = _msgReceived.get(); // Waits forever until it receives a thing
_msgReceived.free((bool *)newEvent.value.p);
if (_inCharIndex == (_MAXCMDLENGTH)) {
_inCharQ[_MAXCMDLENGTH] = '\0'; // Force the string to have an end character
putMessage(error, 1);
_inCharIndex = 0; // Reset buffer index
}
else{
newChar = _inCharQ[_inCharIndex];
if(newChar != '\r'){ // While the command is not over,
_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();
}
}
}
}
//-- Interrupt Service Routine for Serial Port and Character Queue Handling -----------------------------------------//
void serialISR() {
if (_pc.readable()) {
char newChar = _pc.getc();
_inCharQ[_inCharIndex] = newChar; // Save input character
bool *new_msg = _msgReceived.alloc();
*new_msg = true;
_msgReceived.put(new_msg);
}
}
//-- 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
_motor_pos = 0;
if (&_targetVel < (void *)0) {
_targetRot = -100;
} else {
_targetRot = 100;
}
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
_targetVel = 2e3;
_motor_pos = 0;
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;
}
}
//-- Read In Note Data From Serial Port and Parse Into Class Variables ----------------------------------------------//
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 specLen = 2*(len+1) +1; // Add extra character for number of repeats, and +1 for the letter T
bool isChar = true; // After 'T' first is character note
char formatSpec[specLen];
formatSpec[0]='T';
for (int i = 1; i < specLen; 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[specLen] = '\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-1)/2;
_modeBitField = 0x08;
_noteRep = _noteDur[(len-1)/2];
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 mot_orState:
_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);
}
}
//-- Put a Message On the Outgoing Message Stack --------------------------------------------------------------------//
void putMessage(msgType type, uint32_t message){
msg *p_msg = _msgStack.alloc();
p_msg->type = type;
p_msg->message = message;
_msgStack.put(p_msg);
}
//-- Attach CommOut Thread to the Outgoing Communication Function ---------------------------------------------------//
void start_comm(){
_RUN = true;
_tCommOut.start(callback(this, &Comm::commOutFn));
_tCommIn.start(callback(this, &Comm::commInFn));
}
};
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
int8_t old_rotor_state;
uint8_t _theStates[3], // The Key states
_stateCount[3], // State Counter
encState;
uint32_t _mtrPeriod, // Motor period
_MAXPWM_PRD,
quadratureStates,
MINPWM_PRD,
maxEncCount,
encCount,
encTotal,
badEdges;
float _dutyC; // 1 = 100%
bool _RUN;
Comm* _pComm;
Thread _tMotorCtrl; // Thread for motor Control
public:
//-- Default Constructor With Thread Object Constructor -------------------------------------------------------------//
Motor() : _tMotorCtrl(osPriorityAboveNormal2, 2048){
_dutyC = 1.0f; // Set Power to maximum to drive motorHome()
_mtrPeriod = 2e3; // Motor period
pwmCtrl.period_us(_mtrPeriod); // Initialise PWM
pwmCtrl.pulsewidth_us(_mtrPeriod);
_orState = motorHome(); // Rotor offset at motor state 0
_currentState = readRotorState(); // Current Rotor State
_lead = 2; // 2 for forwards, -2 for backwards
old_rotor_state = _orState; // Set old_rotor_state to the origin to begin with
_theStates[0] = _orState +1; // Initialise repeatable states, for us this is state 1
_theStates[1] = (_orState + _lead) % 6 +1; // state 3
_theStates[2] = (_orState + (_lead*2)) % 6 +1; // state 5
_stateCount[0] = 0; // Initialise
_stateCount[1] = 0;
_stateCount[2] = 0;
_pComm = NULL; // Initialise as NULL
_RUN = false;
_MAXPWM_PRD = 2e3;
maxEncCount = 0;
encCount = 0;
encTotal = 0;
badEdges = 0;
quadratureStates = 1112;
}
int findMinTorque() {
int8_t prevState = readRotorState();
uint32_t _mtPeriod = 700;
Timer testTimer;
testTimer.start();
_pComm->_pc.printf("PState:%i, CState:%i\n", prevState, readRotorState());
// stateUpdate();
while (readRotorState() == prevState) {
testTimer.reset();
// pwmCtrl.period_us(2e3);
pwmCtrl.pulsewidth_us(_mtPeriod);
stateUpdate();
while (testTimer.read_ms() < 500) {}
// prevState = readRotorState();
_mtPeriod += 10;
_mtPeriod = _mtPeriod >= 1000 ? 700 : _mtPeriod;
}
_pComm->_pc.printf("Min Torque:%i\n", _mtPeriod);
return _mtPeriod;
}
//-- Start Motor and Attach State Update Function to Rise/Fall Interrupts -------------------------------------------//
void motorStart(Comm *comm) {
I1.fall(callback(this, &Motor::stateUpdate)); // Establish Photo Interrupter Service Routines (auto choose next state)
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));
motorOut((_currentState-_orState+_lead+6)%6); // Push digitally so static motor will start moving
_dutyC = 0.8; // Default a lower duty cycle
pwmCtrl.period_us((uint32_t)_mtrPeriod);
pwmCtrl.pulsewidth_us((uint32_t)(_mtrPeriod*_dutyC));
_pComm = comm;
_RUN = true;
MINPWM_PRD = 920;
_tMotorCtrl.start(callback(this, &Motor::motorCtrlFn)); // Start motor control thread
_pComm->_pc.printf("origin=%i, _theStates=[%i,%i,%i]\n\r", _orState, _theStates[0], _theStates[1], _theStates[2]); // Print information to terminal
}
//-- Set a Predetermined Drive State -------------------------------------------------------------------------------//
void motorOut(int8_t driveState) {
int8_t driveOut = driveTable[driveState & 0x07]; //Lookup the output byte from the drive state
if (~driveOut & 0x01) L1L = 0; //Turn off first
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;
if (driveOut & 0x01) L1L = 1; //Then turn on
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;
}
//-- Inline Conversion of Photointerrupts to a Rotor State ----------------------------------------------------------//
inline int8_t readRotorState() {
return stateMap[I1 + 2*I2 + 4*I3];
}
//-- Basic Motor Stabilisation and Synchronisation ------------------------------------------------------------------//
int8_t motorHome() {
motorOut(0); //Put the motor in drive state 0 and wait for it to stabilise
wait(3.0);
return readRotorState(); //Get the rotor state
}
//-- Motor State Log, Circumvents Issues From Occasionally Skipping Certain States ----------------------------------//
void stateUpdate() { // () { // **params
_currentState = readRotorState(); // Get current state
motorOut((_currentState - _orState + _lead + 6) % 6); // Send the next state to the motor
if (_currentState == 1) {
//if (encCount > maxEncCount) maxEncCount = encCount;
//if (encCount < minEncCount) minEncCount = encCount;
//if (badEdges > maxBadEdges) maxBadEdges = badEdges;
//revCount++;
encTotal += encCount;
encCount = 0;
badEdges = 0;
}
if (_currentState - old_rotor_state == 5) {
_pComm->_motor_pos--;
} else if (_currentState - old_rotor_state == -5) {
_pComm->_motor_pos++;
} else {
_pComm->_motor_pos += (_currentState - old_rotor_state);
}
old_rotor_state = _currentState;
}
// A Rise
void encISR0() {
if (encState == 3) {encCount++;}
else badEdges++;
encState = 0;
}
// B Rise
void encISR1() {
if (encState == 0) {encCount++;}
else badEdges++;
encState = 1;
}
// A Fall
void encISR2() {
if (encState == 1) {encCount++;}
else badEdges++;
encState = 2;
}
// B Fall
void encISR3() {
if (encState == 2) {encCount++;}
else badEdges++;
encState = 3;
}
//-- Motor PID Control ---------------------------------------------------------------------------------------------//
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,
eError = 0; // Rotation error between target and reality
static float rErrorOld; // Old rotation error used for calculation
float encRemain = 0;
bool attached = true;
bool declared = false;
//~~~Controller constants~~~~
int32_t Kp1=88; //22, 27 //Proportional controller constants
int32_t Kp2=26; //12 //Calculated by trial and error to give optimal accuracy
float Kd =34.5; // 40, 14.75
float Kis = 10.0f; // 50
int32_t Kir = 0.0f;
float sIntegral = 0.0f;
float rIntegral = 0.0f;
int32_t Ts; // Initialise controller output Ts (s=speed)
int32_t Tr; // Initialise controller output Tr (r=rotations)
int32_t old_pos = 0,
cur_pos = 0,
sign = 1;
float cur_err = 0.0f,
old_err = 0.0f,
err_diff,
time_diff,
hold_pos = 0,
cur_time = 0.0f,
old_time = 0.0f,
cur_speed = 0.0f;
enum { // To nearest Hz
NOTE_C = 261, // C4
NOTE_D = 293, // D4
NOTE_E = 330, // E4
NOTE_F = 349, // F4
NOTE_G = 392, // G4
NOTE_A = 440, // A4
NOTE_B = 494 // B4
};
m_timer.start();
while (_RUN) {
_tMotorCtrl.signal_wait((int32_t)0x1);
core_util_critical_section_enter(); // Access shared variables here
cpyModeBitfield = _pComm->_modeBitField;
core_util_critical_section_exit();
if ((cpyModeBitfield & 0x01) | (cpyModeBitfield & 0x02)) {// Speed, torque control and PID
if (abs(cur_speed) <= 120 && (!attached)) {
eError = 1.0f;
attached = true;
CHA.rise(callback(this, &Motor::encISR0));
CHB.fall(callback(this, &Motor::encISR3));
CHB.rise(callback(this, &Motor::encISR1));
CHA.fall(callback(this, &Motor::encISR2));
encCount = 0;
encTotal = 0;
encRemain = old_err * quadratureStates; // Number of encs remaining
}
else if (abs(cur_speed) > 120 && (attached)) {
eError = 0.0f;
attached = false;
CHA.rise(NULL);
CHB.fall(NULL);
CHB.rise(NULL);
CHA.fall(NULL);
encCount = 0;
encTotal = 0;
eError = 0;
// Store the number of rotations that have been done up to the point where the channels are activated
hold_pos = (cur_pos / 6.0f);
_pComm->_pc.printf("HPos:%f\n", hold_pos);
}
// read state & timestamp
if (cur_pos < 2) {
rIntegral = 0.0f;
sIntegral = 0.0f;
sign = sgn(_pComm->_targetRot);
}
cur_pos = (_pComm->_motor_pos)*sign; // USE
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 = cur_pos;
// Position error
// if (!attached) {
rError = (_pComm->_targetRot) - (cur_pos/6.0f);
// }
// else{
// rErrorOld = RError;
// rError = (_pComm->_targetRot) - (hold_pos + ((encRemain - encTotal)/quadratureStates));
// }
if ((cur_speed != 0)) {
rIntegral += rError * time_diff;
}
err_diff = rError - old_err;
old_err = rError;
// Speed error - Convert curr_speed from states per time to rotations per time
// TODO: Check the direction that CPos goes in when _targetVel < 0
sError = (_pComm->_targetVel) - (abs(cur_speed/6.0f)); //Read global variable _targetVel updated by interrupt and calculate error between target and reality
if ((cur_speed != 0) && (torque != _MAXPWM_PRD)) {
sIntegral += sError * time_diff;
if (abs(sIntegral * Kis) >= 2000) {
sIntegral -= sError * time_diff;
}
}
Ts = (int32_t)(((Kp1 * sError * sign) + (Kis * sIntegral * sign))); // * sgn(cur_pos));
Tr = (int32_t)((Kp2*rError*sign) + (Kd/time_diff) * err_diff * sign);
if (cpyModeBitfield & 0x01) {
// Speed control
if (_pComm->_targetVel == 0) { //Check if user entered V0,
torque = _MAXPWM_PRD; //and set the output to maximum as specified
}
else {
// select minimum absolute value torque
// if (cur_speed < 0) {
// torque = max(Ts, Tr);
// }
// else {
// torque = min(Ts, Tr);
// }
torque = Ts;
if (abs(sError) > 0.6) {
// torque = abs(Tr) < 1000 ? 1000*sgn(Tr) : Tr;
torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque;
// _pComm->_pc.printf("Speed:%f\r\n", (cur_speed/6.0f));
} else {
torque = 0;
}
torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque;
}
}
else if (cpyModeBitfield & 0x02) {
// select minimum absolute value torque
if (_pComm->_targetRot == 0) {
// Not spinning at max speed
torque = 0.7 * _MAXPWM_PRD;
}
else {
// select minimum absolute value torque
// if (cur_speed < 0) {
// torque = max(Ts, Tr);
// } else {
// torque = min(Ts, Tr);
// }
torque = Tr;
if (abs(rError) > 0.3) {
torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque;
} else {
torque = 0;
if (!declared) {
_pComm->_pc.printf("Remaining Turns:%f\n", rError);
declared = true;
}
//attached = false;
}
}
}
if (torque < 0) {
torque = -torque;
_lead = -2;
} else {
_lead = 2;
}
torque = torque > _MAXPWM_PRD ? _MAXPWM_PRD : torque; // Set a cap on torque
_pComm->_motorTorque = torque;
pwmCtrl.pulsewidth_us(torque);
// _pComm->_pc.printf("EError:%.4f, tot:%d, encRemain:%.4f || RError:%.4f || SError:%.4f, CSpeed:%f, Torque:%i\r\n", eError, encTotal, encRemain, rError, sError, (cur_speed/6.0f), torque);
//_pComm->_pc.printf("EError:%.4f, remain:%.4f, tot%d || RError:%.4f || Torque:%i \r\n", eError, encRemain, encTotal, rError, torque);
// _pComm->_pc.printf("RError:%.4f\r\n", rError);
// Give the motor a kick
stateUpdate();
}
else if (cpyModeBitfield & 0x04) { // If it is in torque mode, do no PID math, just set pulsewidth
torque = (int32_t)_pComm->_motorTorque;
if (oldTorque != torque) {
_pComm->putMessage((Comm::msgType)8, torque);
oldTorque = torque;
}
}
else if (cpyModeBitfield & 0x08) { // If it is in Melody mode
uint32_t oldMtrPeriod = _mtrPeriod; // Backup of current motor period
float oldDutyC = _dutyC; // Backup of current duty cycle
uint32_t noteFreq, newDur, newPeriod =0,
oldPeriod = _mtrPeriod;
Timer testTimer;
testTimer.start();
_pComm->_pc.printf("rep:%i, len:%i \n\r", _pComm->_noteRep, _pComm->_noteLen);
for (int k = 0; k < _pComm->_noteRep; ++k) {
for (int i = 0; i < _pComm->_noteLen; ++i) {
noteFreq = 0;
newDur = (uint32_t)_pComm->_noteDur[i]*1e3; // ms
switch (_pComm->_notes[i]) {
case 'C':
noteFreq = NOTE_C;
break;
case 'D':
noteFreq = NOTE_D;
break;
case 'E':
noteFreq = NOTE_E;
break;
case 'F':
noteFreq = NOTE_F;
break;
case 'G':
noteFreq = NOTE_G;
break;
case 'A':
noteFreq = NOTE_A;
break;
case 'B':
noteFreq = NOTE_B;
break;
default:
break;
}
if (noteFreq == 0) {break;} // leave the loop if we get a wrong character
newPeriod = (uint32_t)(1e6/(uint32_t)noteFreq); // us
if (newPeriod>oldPeriod) { // Change Period First
// _pComm->_pc.printf("Period:changed \n" );
pwmCtrl.period_us(newPeriod); //set frequency of PWM
pwmCtrl.pulsewidth_us((uint32_t)(newPeriod*0.8));
} else { // Change Pulse WidthFirst
pwmCtrl.pulsewidth_us((uint32_t)(newPeriod*0.8));
pwmCtrl.period_us(newPeriod); //set frequency of PWM
}
// stateUpdate();
oldPeriod = newPeriod;
testTimer.reset();
while (testTimer.read_ms() < newDur ) {} // Do nothing
// _pComm->_pc.printf("Period:%d Dur:%d \n",newPeriod, newDur );
}
_pComm->_modeBitField = 0x04; // stop melody mode
}
// After playing notes, go back to the old speed
torque = (int32_t)_pComm->_motorTorque;
pwmCtrl.pulsewidth_us((uint32_t)(torque*0.8));
// And reset the motor period and duty cycle
}
else{
torque = _MAXPWM_PRD * 0.5; // Run at 50% duty cycle if argument not properly defined
}
}
}
void motorCtrlTick(){
_tMotorCtrl.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;
}