Skad00sh
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Revision 33:f1dc3b160eac, committed 2019-03-18
- Comitter:
- CallumAlder
- Date:
- Mon Mar 18 18:10:10 2019 +0000
- Parent:
- 32:fc5e00d9f74d
- Child:
- 34:2c6f635cc8e7
- Commit message:
- House keeping and motor class tidy;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Mar 18 18:04:29 2019 +0000
+++ b/main.cpp Mon Mar 18 18:10:10 2019 +0000
@@ -72,420 +72,416 @@
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 T_*/{
-
-public:
-
- 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;
-
- enum msgType {motorState, posIn, velIn, posOut, velOut,
-
- hashRate, keyAdded, nonceMatch,
+class Comm{
- torque, rotations,
-
- error};
-
- typedef struct {
- msgType type;
- uint32_t message;
- } msg;
-
- Mail<msg, 32> mailStack;
-
- void serialISR(){
- 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);
- 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[inCharQIdx] = newChar; //save input character and
- inCharQIdx++; //advance index
- pc.putc(newChar);
+ public:
+
+ 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;
+
+ enum msgType {motorState, posIn, velIn, posOut, velOut,
+
+ hashRate, keyAdded, nonceMatch,
+
+ torque, rotations,
+
+ error};
+
+ typedef struct {
+ msgType type;
+ uint32_t message;
+ } msg;
+
+ Mail<msg, 32> mailStack;
+
+ void serialISR(){
+ 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);
+ 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{
- 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[inCharQIdx] = newChar; //save input character and
+ inCharQIdx++; //advance index
+ pc.putc(newChar);
+ }
+ 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
+ }
}
}
+
+
+ }
+
+ 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('<');
+ }
+
+ 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;
+ }
}
-
-
- }
-
- 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('<');
- }
-
- 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;
- }
- }
-
- //~~~~~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);
- 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);
- 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);
- 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);
+ 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);
+ 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);
+ break;
+ }
+ mailStack.free(pMessage);
}
- 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("%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('.');
+
+
+
+
+ //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("%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');
+
+ inCharQ[MAXCMDLENGTH] = '\0';
+ strncpy(newCmd, inCharQ, MAXCMDLENGTH);
+
+ cmdIndx = 0;
+
+ inCharQIdx = 0;
+ // inCharQIdx = MAXCMDLENGTH-1;
+
+
+
+ pc.attach(callback(this, &Comm::serialISR));
+
+ // Thread t_comm_in(osPriorityAboveNormal, 1024);
+ // Thread t_comm_out(osPriorityAboveNormal, 1024);
+ // Thread t_motor_ctrl(osPriorityAboveNormal, 1024);
+
+ motorPower = 300;
+ targetVel = 45.0;
+ targetRot = 459.0;
+
+
+
+ /*MsgChar = {'m', 'R', 'V', 'r', 'v',
+
+ 'h', 'K', 'n',
+
+ 'T', 'r',
+
+ 'e'};*/
}
- pc.putc('<');
- pc.putc('\r');
-
- inCharQ[MAXCMDLENGTH] = '\0';
- strncpy(newCmd, inCharQ, MAXCMDLENGTH);
-
- cmdIndx = 0;
-
- inCharQIdx = 0;
- // inCharQIdx = MAXCMDLENGTH-1;
-
-
-
- pc.attach(callback(this, &Comm::serialISR));
-
- // Thread t_comm_in(osPriorityAboveNormal, 1024);
- // Thread t_comm_out(osPriorityAboveNormal, 1024);
- // Thread t_motor_ctrl(osPriorityAboveNormal, 1024);
-
- motorPower = 300;
- targetVel = 45.0;
- targetRot = 459.0;
-
-
-
- /*MsgChar = {'m', 'R', 'V', 'r', 'v',
-
- 'h', 'K', 'n',
-
- 'T', 'r',
-
- 'e'};*/
- }
-
-
- 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;
-
-
- // 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('.');
+
+
+ void putMessage(msgType type, uint32_t message){
+ msg *p_msg = mailStack.alloc();
+ p_msg->type = type;
+ p_msg->message = message;
+ mailStack.put(p_msg);
}
- pc.putc('<');
- pc.putc('\r');
-
- 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));
-
-
-
- }
-
- char newCmd[]; // because unallocated must be defined at the bottom of the class
- char inCharQ[];
+
+ void start_comm(){
+ _RUN = true;
+
+
+ // 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();
+
+ // t_comm_in.start(callback(this, &Comm::commInFn));
+ // this::thread::wait()
+ // wait(1.0);
+ t_comm_out.start(callback(this, &Comm::commOutFn));
+
+
+
+ }
+
+ char newCmd[]; // because unallocated must be defined at the bottom of the class
+ char inCharQ[];
};
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
-
- //Phase lead to make motor spin
- int8_t lead;
-
- Comm* p_comm;
-
- //Run the motor synchronisation
+ 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
- float dutyC; // 1 = 100%
- float mtrPeriod; // motor period
- uint8_t stateCount[3]; // State Counter
- uint8_t theStates[3]; // The Key states
-
- Thread t_motor_ctrl; // Thread for motor Control
-
-public:
-
- Motor() : t_motor_ctrl(osPriorityAboveNormal, 1024)
- {
- // Set Power to maximum to drive motorHome()
- dutyC = 1;
- mtrPeriod = 2e-3; // motor period
- pwmCtrl.period(mtrPeriod);
- pwmCtrl.pulsewidth(mtrPeriod*dutyC);
-
- 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
-
- theStates[0] = orState;
- theStates[1] = (orState + lead) % 6;
- theStates[2] = (orState + (lead*2)) % 6;
-
- lead = 2; //2 for forwards, -2 for backwards
-
- stateCount[0] = 0; stateCount[1] = 0; stateCount[2] = 0;
- theStates[0] = 0; theStates[1] = 0; theStates[2] = 0;
+ //Phase lead to make motor spin
+ int8_t lead;
+
+ Comm* p_comm;
+
+ //Run the motor synchronisation
- p_comm = NULL; // null pointer for now
-
- }
-
-
- 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(mtrPeriod);
- pwmCtrl.pulsewidth(mtrPeriod*dutyC);
-
- // Start motor control thread
- t_motor_ctrl.start(callback(this, &Motor::motorCtrlFn));
-
- p_comm = comm;
- }
-
- //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(2.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;
- motorCtrlTicker.attach_us(callback(this,&Motor::motorCtrlTick), 1e5);
- while (1) {
- t_motor_ctrl.signal_wait((int32_t)0x1);
- p_comm->pc.printf("B4115");
+ float dutyC; // 1 = 100%
+ float mtrPeriod; // motor period
+ uint8_t stateCount[3]; // State Counter
+ uint8_t theStates[3]; // The Key states
+
+ Thread t_motor_ctrl; // Thread for motor Control
+
+ public:
+
+ Motor() : t_motor_ctrl(osPriorityAboveNormal, 1024)
+ {
+ // Set Power to maximum to drive motorHome()
+ dutyC = 1;
+ mtrPeriod = 2e-3; // motor period
+ pwmCtrl.period(mtrPeriod);
+ pwmCtrl.pulsewidth(mtrPeriod*dutyC);
+
+ 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
+
+ theStates[0] = orState;
+ theStates[1] = (orState + lead) % 6;
+ theStates[2] = (orState + (lead*2)) % 6;
+
+ lead = 2; //2 for forwards, -2 for backwards
+
+ stateCount[0] = 0; stateCount[1] = 0; stateCount[2] = 0;
+ theStates[0] = 0; theStates[1] = 0; theStates[2] = 0;
+
+ p_comm = NULL; // null pointer for now
+
+ }
+
+
+ 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(mtrPeriod);
+ pwmCtrl.pulsewidth(mtrPeriod*dutyC);
+
+ // Start motor control thread
+ t_motor_ctrl.start(callback(this, &Motor::motorCtrlFn));
+
+ p_comm = comm;
}
- }
-
- void motorCtrlTick(){
- t_motor_ctrl.signal_set(0x1);
- }
+
+ //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(2.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;
+ motorCtrlTicker.attach_us(callback(this,&Motor::motorCtrlTick), 1e5);
+ while (1) {
+ t_motor_ctrl.signal_wait((int32_t)0x1);
+ p_comm->pc.printf("B4115");
+ }
+ }
+
+ void motorCtrlTick(){
+ t_motor_ctrl.signal_set(0x1);
+ }
};
@@ -546,4 +542,7 @@
timer.reset();
}
}
+
+ return 0;
+
}
\ No newline at end of file