SpinnyBoi
First Commit
Dependencies: mbed Crypto_light mbed-rtos
Spin it 2 win it
Diff: main.cpp
- Revision:
- 27:d50f1914f23a
- Parent:
- 25:bf5fff055aef
- Child:
- 28:8076013fbef5
--- a/main.cpp Tue Mar 20 18:59:22 2018 +0000
+++ b/main.cpp Fri Mar 23 16:35:17 2018 +0000
@@ -19,241 +19,371 @@
#define L3Lpin D9 //0x10
#define L3Hpin D10 //0x20
-#define CHAR_ARR_SIZE 18 //Max length of input codes
+// max input length
+#define CHAR_ARR_SIZE 18
+
+// pwm/motor control definitions
#define MAX_PWM_PERIOD 2000
#define MAX_TORQUE 1000
-#define KP 25
+#define KP 20
+#define KD 20
-//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 - - -
-*/
-//Drive state to output table
-const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
+// function-like macros 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 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
-
-//Phase lead to make motor spin
-const int8_t lead = 2; //2 for forwards, -2 for backwards
-
-//Rotor offset at motor state 0
-volatile int8_t orState = 0;
-
-//Set initial torque of 1000
-volatile int32_t torque = 1000;
+enum MSG {MSG_RESET, MSG_HASHCOUNT, MSG_NONCE_OK, MSG_OVERFLOW, MSG_ROT_PEN,
+ MSG_MAX_SPD, MSG_NEW_KEY, MSG_INP_ERR, MSG_TORQUE, MSG_TEST,
+ MSG_CUR_SPD, MSG_POS, MSG_NEW_VEL, MSG_NEW_ROTOR_POS};
-enum MSG {MSG_RESET, MSG_HASHCOUNT, MSG_NONCE_OK,
- MSG_OVERFLOW, MSG_ROT_PEN, MSG_MAX_SPD, MSG_NEW_KEY, MSG_INP_ERR, MSG_TORQUE,
- MSG_TEST, MSG_CUR_SPD};
-
-//Instantiate the serial port
+// Instantiate the serial port
RawSerial pc(SERIAL_TX, SERIAL_RX);
-//Status LED
+// Status LED
DigitalOut led1(LED1);
-//Photointerrupter inputs
+// Photointerrupter inputs
InterruptIn I1(I1pin);
InterruptIn I2(I2pin);
InterruptIn I3(I3pin);
-//Motor Drive outputs
+// motor drive outputs
PwmOut L1L(L1Lpin);
+PwmOut L2L(L2Lpin);
+PwmOut L3L(L3Lpin);
DigitalOut L1H(L1Hpin);
-PwmOut L2L(L2Lpin);
DigitalOut L2H(L2Hpin);
-PwmOut L3L(L3Lpin);
DigitalOut L3H(L3Hpin);
+// givens from coursework handouts - motor states etc
+const int8_t drive_table[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
+const int8_t state_map[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
+volatile int8_t lead = 2, // phase lead, -2 for backwards, 2 for forwards
+ origin_state = 0;
+// threads for serial I/O and motor control
+Thread comms_out_thrd(osPriorityNormal, 1024);
+Thread comms_in_thrd(osPriorityNormal, 1024);
+Thread motor_ctrl_thrd(osPriorityNormal, 2048);
+
+// IPC via Mail object; we instantiate here
typedef struct {
+ char *stub;
uint8_t code;
int32_t data;
-} message_t ;
+} message_t;
+
+Mail<message_t, 16> msg_out_queue;
+
+// mutex variables
+Mutex new_key_mutex;
+Mutex target_speed_mutex;
+Mutex rotations_pending_mutex;
+
+// instantiate a queue to buffer incoming characters
+Queue<void, 8> serial_in_queue;
+// motor control global variables
+volatile int32_t motor_position = 0,
+ target_speed = 256,
+ torque = 1000;
+volatile float rotations_pending = 0;
+
+// hash count, reset every second when printed
+volatile uint16_t hashcount = 0;
+// used when selecting a new hash key
+volatile uint64_t new_key = 0;
+
+// logging function & shim macro for stringifying enum
+#define put_message(code, data) put_message_(#code, code, data)
+void put_message_(char *, uint8_t, int32_t);
-Mail<message_t,16> outMessages;
+void comms_out_fn(void); // serial output thread main
+void comms_in_fn(void); // serial input thread main
+void serial_isr(void); // serial event ISR
+void photointerrupter_isr(void); // motor state change ISR
+void motor_ctrl_fn(void); // motor control thread main
+void motor_ctrl_timer_isr(void); // poke motor_ctrl at 100ms intervals
+void parse_serial_in(char *); // interpret serial command
+void do_hashcount(void); // print current hash count
+inline int8_t read_rotor_state(void); // get rotor position
+int8_t motor_home(void); // establish motor origin position
+void motor_out(int8_t, uint32_t); // do motor output
+
+int main(void)
+{
+
+ comms_out_thrd.start(&comms_out_fn);
+ motor_ctrl_thrd.start(&motor_ctrl_fn);
+ comms_in_thrd.start(&comms_in_fn);
+
+ put_message(MSG_RESET, 0);
+
+ // sync motor to home
+ rotations_pending = origin_state = motor_home();
+
+ // register ISRs
+ I1.rise(&photointerrupter_isr);
+ I2.rise(&photointerrupter_isr);
+ I3.rise(&photointerrupter_isr);
+
+ I1.fall(&photointerrupter_isr);
+ I2.fall(&photointerrupter_isr);
+ I3.fall(&photointerrupter_isr);
-void putMessage(uint8_t code, int32_t data) {//uint64_t
- message_t *pMessage = outMessages.alloc();
- pMessage->code = code;
- pMessage->data = data;
- outMessages.put(pMessage);
+ // set PWM period
+ L1L.period_us(MAX_PWM_PERIOD);
+ L2L.period_us(MAX_PWM_PERIOD);
+ L3L.period_us(MAX_PWM_PERIOD);
+
+ //Calling the ISR once starts the motor movement
+ photointerrupter_isr();
+
+ // SHA256-related data
+ SHA256 sha256;
+ 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};
+ uint64_t* key = (uint64_t*)((int)sequence + 48);
+ uint64_t* nonce = (uint64_t*)((int)sequence + 56);
+ uint8_t hash[32];
+
+ Ticker hashcounter;
+ hashcounter.attach(&do_hashcount, 1.0);
+
+ //Poll the rotor state and set the motor outputs accordingly to spin the motor
+ while (1) {
+ // compute new hash
+ *key = new_key;
+ sha256.computeHash(hash, sequence, 64);
+
+ if (hash[0] == 0 && hash[1] == 0)
+ put_message(MSG_NONCE_OK, *nonce);
+
+ (*nonce)++;
+ hashcount++;
+ }
}
-Thread commOutT (osPriorityNormal, 1024);
+void put_message_(char *str, uint8_t code, int32_t data)
+{
+ message_t *message = msg_out_queue.alloc();
+ message->code = code;
+ message->data = data;
+ message->stub = str;
+ msg_out_queue.put(message);
+}
-void commOutFn() {
+void comms_out_fn()
+{
while(1) {
- osEvent newEvent = outMessages.get();
- message_t *pMessage = (message_t*)newEvent.value.p;
- pc.printf("Message: [%d], data: 0x%08x\r\n",
- pMessage->code,pMessage->data);
- outMessages.free(pMessage);
+ osEvent new_event = msg_out_queue.get();
+ message_t *message = (message_t*) new_event.value.p;
+ pc.printf("[%16s], data: %010d\r\n",
+ message->stub,
+ message->data);
+ msg_out_queue.free(message);
}
}
-
-
-//Global varible for the Bitcoin Key,targetSpeed and rotations_pending
-volatile uint64_t newKey = 0; //check initialise value? ****
-volatile float targetSpeed = 0, rotations_pending = 0;
-//mutex variables
-Mutex newKey_mutex;
-Mutex targetSpeed_mutex;
-Mutex rotations_pending_mutex;
+//serial port ISR to receive each incoming byte and place into queue
+void serial_isr()
+{
+ uint8_t new_char = pc.getc();
+ serial_in_queue.put((void*) new_char);
+}
-//Instantiate a Queue to buffer incoming characters
-Queue<void, 8> inCharQ;
-//serial port ISR to receive each incoming byte and place into queue
-void serialISR() {
- uint8_t newChar = pc.getc();
- inCharQ.put((void*)newChar);
-}
-Thread motorCtrlT (osPriorityNormal, 2048);
+// photointerrupter ISR drives the motors
+void photointerrupter_isr()
+{
+ static int8_t old_rotor_state = 0;
+ int8_t rotor_state = read_rotor_state();
+
+ motor_out((rotor_state-origin_state+lead+6)%6, torque); //+6 to make sure the remainder is positive
-int8_t readRotorState();
-void motorOut(int8_t,uint32_t);
-volatile int32_t motorPosition;
-
-void photointerrupter_isr() {
- static int8_t oldRotorState = 0;
- int8_t rotorState = readRotorState();
- motorOut((rotorState-orState+lead+6)%6, torque); //+6 to make sure the remainder is positive
- if (rotorState - oldRotorState == 5) motorPosition--;
- else if (rotorState - oldRotorState == -5) motorPosition++;
- else motorPosition += (rotorState - oldRotorState);
- oldRotorState = rotorState;
+ if (rotor_state - old_rotor_state == 5)
+ motor_position--;
+ else if (rotor_state - old_rotor_state == -5)
+ motor_position++;
+ else
+ motor_position += (rotor_state - old_rotor_state);
+
+ old_rotor_state = rotor_state;
}
-//TODO Implement timer for greater accuracy
-void motorCtrlTick(){
- motorCtrlT.signal_set(0x1);
-}
+// motor control thread sets a timer ISR, this is the handler
+void motor_ctrl_timer_isr() { motor_ctrl_thrd.signal_set(0x1); }
-volatile float curSpeed = 0;
+// motor control thread main
+void motor_ctrl_fn()
+{
+ Ticker motor_control_ticker;
+ Timer timer;
+
+ uint8_t count = 0;
+ int32_t cur_pos = 0,
+ old_pos = 0,
+ cur_speed,
+ ys,
+ yr;
-void motorCtrlFn(){
- static int32_t curPos = 0, oldPos = 0;
- Ticker motorCtrlTicker;
- motorCtrlTicker.attach_us(&motorCtrlTick,100000);
- uint8_t count = 0;
- Timer timer;
+ uint32_t cur_time = 0,
+ old_time = 0,
+ time_diff;
+
+ float cur_err = 0.0f,
+ old_err = 0.0f,
+ err_diff;
+
+ motor_control_ticker.attach_us(&motor_ctrl_timer_isr,100000);
+
timer.start();
- uint32_t curTime = 0, oldTime = 0, timeDiff;
+
+ while(1) {
+ // wait for the 100ms boundary
+ motor_ctrl_thrd.signal_wait(0x1);
+
+ // read state & timestamp
+ cur_time = timer.read();
+ cur_pos = motor_position;
+
+ // 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;
+
+ count = ++count % 10;
- while(1){
- motorCtrlT.signal_wait(0x1);
- curTime = timer.read();
- timeDiff = curTime - oldTime;
- oldTime = curTime;
- curPos = motorPosition;
- curSpeed = (curPos - oldPos) / timeDiff;
- oldPos = curPos;
- count = (count+1)%10;
- if (!count) {
- putMessage(MSG_CUR_SPD, torque);
- }
- torque = (int32_t)(KP*(targetSpeed-abs(curSpeed)));
+ // update with motor status
+ /*
+ * if (!count) {
+ * put_message(MSG_CUR_SPD, cur_speed);
+ * put_message(MSG_MAX_SPD, target_speed);
+ * put_message(MSG_POS, (cur_pos/6));
+ * put_message(MSG_ROT_PEN, rotations_pending);
+ * put_message(MSG_TORQUE, torque);
+ * }
+ */
+
+ // compute position error
+ cur_err = rotations_pending - (cur_pos/6.0f);
+ err_diff = cur_err - old_err;
+ old_err = cur_err;
+
+ // compute torques
+ ys = (int32_t) (20 * (target_speed - abs(cur_speed))) * sgn(cur_err);
+ yr = (int32_t) ((20 * cur_err) + (40 * err_diff));
+
+ // select minimum absolute value torque
+ if (cur_speed < 0)
+ torque = max(ys, yr);
+ else
+ torque = min(ys, yr);
+
+ // fix torque if negative
+ if (torque < 0)
+ torque = -torque, // <- comma operator in action
+ lead = -2;
+ else
+ lead = 2;
+
+ // cap torque
+ if (torque > MAX_TORQUE)
+ torque = MAX_TORQUE;
+
+ // finally, give the motor a kick
+ photointerrupter_isr();
}
}
-
-
-void setNewCmd(char s[CHAR_ARR_SIZE])
+// parse input with sscanf()
+void parse_serial_in(char *s)
{
- uint64_t newKey_;
- int32_t torque_;
- float targetSpeed_, rotations_pending_;
- //R
- if (sscanf(s, "R%f", &rotations_pending_)) {
- rotations_pending_mutex.lock();
- rotations_pending = rotations_pending_;
- rotations_pending_mutex.unlock();
- putMessage(MSG_ROT_PEN, rotations_pending);
- //V
- }
- else if (sscanf(s, "V%f", &targetSpeed_)) {
- targetSpeed_mutex.lock();
- targetSpeed = targetSpeed_;
- targetSpeed_mutex.unlock();
- putMessage(MSG_MAX_SPD, targetSpeed);
- //K
- }
- else if (sscanf(s, "K%llx", &newKey_)) {
- newKey_mutex.lock();
- newKey = newKey_;
- newKey_mutex.unlock();
- putMessage(MSG_NEW_KEY, newKey);
- }
- else if (sscanf(s, "T%u", &torque_)) {
- torque = torque_;
- photointerrupter_isr(); //Give it a kick
- putMessage(MSG_TORQUE, torque);
- //ERROR
- }
- else{
- putMessage(MSG_INP_ERR, 0x404);
- }
+ // shadow output variables so writes are guaranteed atomic
+ uint64_t new_key_;
+ int32_t torque_;
+ int32_t target_speed_;
+ float rotations_pending_;
+
+ if (sscanf(s, "R%f", &rotations_pending_)) {
+
+ rotations_pending += rotations_pending_;
+ //put_message(MSG_ROT_PEN, rotations_pending);
+
+ } else if (sscanf(s, "V%d", &target_speed_)) {
+
+ target_speed_mutex.lock();
+ target_speed = target_speed_;
+ target_speed_mutex.unlock();
+ //put_message(MSG_NEW_VEL, target_speed);
+
+ } else if (sscanf(s, "K%llx", &new_key_)) {
+
+ new_key_mutex.lock();
+ new_key = new_key_;
+ new_key_mutex.unlock();
+ //put_message(MSG_NEW_KEY, new_key);
+
+ } else if (sscanf(s, "T%u", &torque_)) {
+ torque = torque_;
+ photointerrupter_isr(); //Give it a kick
+ //put_message(MSG_TORQUE, torque);
+ } else
+ put_message(MSG_INP_ERR, 0x404);
}
-Thread decodeT (osPriorityNormal, 512);
-
-void decodeFn() {
- pc.attach(&serialISR);
- char charSeq[CHAR_ARR_SIZE] = "";
- uint8_t bufPos = 0;
- while(1) {
+void comms_in_fn()
+{
+ // register serial interrupt handler
+ pc.attach(&serial_isr);
- if(bufPos >= CHAR_ARR_SIZE) {
- putMessage(MSG_OVERFLOW, bufPos);
- bufPos = 0;
- }
- else{
-
- osEvent newEvent = inCharQ.get();
- uint8_t newChar = (uint8_t)newEvent.value.p;
+ char char_seq[CHAR_ARR_SIZE] = "";
+ uint8_t buf_pos = 0;
+
+ while (1) {
+ if (buf_pos >= CHAR_ARR_SIZE) {
+ put_message(MSG_OVERFLOW, buf_pos);
+ buf_pos = 0;
+ } else {
+ osEvent new_event = serial_in_queue.get();
+ uint8_t new_char = (uint8_t)new_event.value.p;
- if(newChar == '\r' || newChar == '\n') {
- charSeq[bufPos] = '\0';
- bufPos = 0;
- setNewCmd(charSeq);
- }
- else {
- charSeq[bufPos] = newChar;
- bufPos++;
- }
+ if (new_char == '\r' || new_char == '\n') {
+ char_seq[buf_pos] = '\0';
+ buf_pos = 0;
+ parse_serial_in(char_seq);
+ } else
+ char_seq[buf_pos++] = new_char;
}
}
}
-volatile uint16_t hashcount = 0;
-
-void do_hashcount() {
- //putMessage(MSG_HASHCOUNT, hashcount);
+// timer ISR to print/reset hash counts every second
+void do_hashcount()
+{
+ put_message(MSG_HASHCOUNT, hashcount);
hashcount = 0;
}
//Set a given drive state
-void motorOut(int8_t driveState, uint32_t t){
+void motor_out(int8_t driveState, uint32_t t){
//Lookup the output byte from the drive state.
- int8_t driveOut = driveTable[driveState & 0x07];
+ int8_t driveOut = drive_table[driveState & 0x07];
//Turn off first
if (~driveOut & 0x01) L1L.pulsewidth_us(0);
@@ -272,86 +402,19 @@
if (driveOut & 0x20) L3H = 0;
}
- //Convert photointerrupter inputs to a rotor state
-inline int8_t readRotorState(){
- return stateMap[I1 + 2*I2 + 4*I3];
+//Convert photointerrupter inputs to a rotor state
+inline int8_t read_rotor_state()
+{
+ return state_map[I1 + 2*I2 + 4*I3];
}
//Basic synchronisation routine
-int8_t motorHome() {
+int8_t motor_home()
+{
//Put the motor in drive state 0 and wait for it to stabilise
- motorOut(0, MAX_TORQUE);
+ motor_out(0, MAX_TORQUE);
wait(2.0);
//Get the rotor state
- return readRotorState();
-}
-
-
-
-//Main
-int main() {
-
- putMessage(MSG_RESET, 0);
-
- commOutT.start(&commOutFn);
-
- motorCtrlT.start(&motorCtrlFn);
-
- decodeT.start(&decodeFn);
-
- //Run the motor synchronisation
- orState = motorHome();
-
- //orState is subtracted from future rotor state inputs to align rotor and motor states
-
- I1.rise(&photointerrupter_isr);
- I2.rise(&photointerrupter_isr);
- I3.rise(&photointerrupter_isr);
-
- I1.fall(&photointerrupter_isr);
- I2.fall(&photointerrupter_isr);
- I3.fall(&photointerrupter_isr);
-
-
- L1L.period_us(MAX_PWM_PERIOD);
- L2L.period_us(MAX_PWM_PERIOD);
- L3L.period_us(MAX_PWM_PERIOD);
-
- //Calling the ISR once starts the motor movement
- photointerrupter_isr();
-
-
-
- SHA256 sha256;
-
- 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};
- uint64_t* key = (uint64_t*)((int)sequence + 48);
- uint64_t* nonce = (uint64_t*)((int)sequence + 56);
- uint8_t hash[32];
-
- Ticker hashcounter;
- hashcounter.attach(&do_hashcount, 1.0);
-
- //Poll the rotor state and set the motor outputs accordingly to spin the motor
- while (1) {
-
- *key = newKey;
-
- sha256.computeHash(hash, sequence, 64);
-
- if (hash[0] == 0 && hash[1] == 0) {
- //putMessage(MSG_NONCE_OK, *nonce);
- }
-
- (*nonce)++;
- hashcount++;
- }
-}
+ return read_rotor_state();
+}
\ No newline at end of file