First Commit
Dependencies: mbed Crypto_light mbed-rtos
Spin it 2 win it
Diff: main.cpp
- Revision:
- 26:1a7a154e375b
- Parent:
- 25:bf5fff055aef
diff -r bf5fff055aef -r 1a7a154e375b main.cpp --- a/main.cpp Tue Mar 20 18:59:22 2018 +0000 +++ b/main.cpp Wed Mar 21 12:34:39 2018 +0000 @@ -2,16 +2,16 @@ #include "Crypto_light/hash/SHA256.h" #include "mbed-rtos/rtos/rtos.h" -//Photointerrupter input pins +// Photointerrupter input pins #define I1pin D2 #define I2pin D11 #define I3pin D12 -//Incremental encoder input pins +// Incremental encoder input pins #define CHA D7 #define CHB D8 -//Motor Drive output pins //Mask in output byte +// Motor Drive output pins //Mask in output byte #define L1Lpin D4 //0x01 #define L1Hpin D5 //0x02 #define L2Lpin D3 //0x04 @@ -19,292 +19,113 @@ #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); - - - -typedef struct { - uint8_t code; - int32_t data; -} message_t ; - -Mail<message_t,16> outMessages; - - -void putMessage(uint8_t code, int32_t data) {//uint64_t - message_t *pMessage = outMessages.alloc(); - pMessage->code = code; - pMessage->data = data; - outMessages.put(pMessage); -} - -Thread commOutT (osPriorityNormal, 1024); - -void commOutFn() { - 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); - } -} - +// 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; -//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; - -//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); - -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; -} - -//TODO Implement timer for greater accuracy -void motorCtrlTick(){ - motorCtrlT.signal_set(0x1); -} - -volatile float curSpeed = 0; - - -void motorCtrlFn(){ - static int32_t curPos = 0, oldPos = 0; - Ticker motorCtrlTicker; - motorCtrlTicker.attach_us(&motorCtrlTick,100000); - uint8_t count = 0; - Timer timer; - timer.start(); - uint32_t curTime = 0, oldTime = 0, timeDiff; - - 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))); - } -} - +// 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); -void setNewCmd(char s[CHAR_ARR_SIZE]) -{ - 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); - } -} +// IPC via Mail object; we instantiate here +typedef struct { + char *stub; + uint8_t code; + int32_t data; +} message_t; + +Mail<message_t, 16> msg_out_queue; -Thread decodeT (osPriorityNormal, 512); - -void decodeFn() { - pc.attach(&serialISR); - char charSeq[CHAR_ARR_SIZE] = ""; - uint8_t bufPos = 0; - while(1) { +// mutex variables +Mutex new_key_mutex; +Mutex target_speed_mutex; +Mutex rotations_pending_mutex; - if(bufPos >= CHAR_ARR_SIZE) { - putMessage(MSG_OVERFLOW, bufPos); - bufPos = 0; - } - else{ +// 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; - osEvent newEvent = inCharQ.get(); - uint8_t newChar = (uint8_t)newEvent.value.p; +// 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; - if(newChar == '\r' || newChar == '\n') { - charSeq[bufPos] = '\0'; - bufPos = 0; - setNewCmd(charSeq); - } - else { - charSeq[bufPos] = newChar; - bufPos++; - } - } - } -} - -volatile uint16_t hashcount = 0; - -void do_hashcount() { - //putMessage(MSG_HASHCOUNT, hashcount); - hashcount = 0; -} - - -//Set a given drive state -void motorOut(int8_t driveState, uint32_t t){ - - //Lookup the output byte from the drive state. - int8_t driveOut = driveTable[driveState & 0x07]; - - //Turn off first - if (~driveOut & 0x01) L1L.pulsewidth_us(0); - if (~driveOut & 0x02) L1H = 1; - if (~driveOut & 0x04) L2L.pulsewidth_us(0); - if (~driveOut & 0x08) L2H = 1; - if (~driveOut & 0x10) L3L.pulsewidth_us(0); - if (~driveOut & 0x20) L3H = 1; +// 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); - //Then turn on - if (driveOut & 0x01) L1L.pulsewidth_us(t); - if (driveOut & 0x02) L1H = 0; - if (driveOut & 0x04) L2L.pulsewidth_us(t); - if (driveOut & 0x08) L2H = 0; - if (driveOut & 0x10) L3L.pulsewidth_us(t); - 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, MAX_TORQUE); - wait(2.0); +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 - //Get the rotor state - return readRotorState(); -} - +int main(void) +{ - -//Main -int main() { - - putMessage(MSG_RESET, 0); + comms_out_thrd.start(&comms_out_fn); + motor_ctrl_thrd.start(&motor_ctrl_fn); + comms_in_thrd.start(&comms_in_fn); - commOutT.start(&commOutFn); - - motorCtrlT.start(&motorCtrlFn); - - decodeT.start(&decodeFn); + put_message(MSG_RESET, 0); - //Run the motor synchronisation - orState = motorHome(); + // sync motor to home + rotations_pending = origin_state = motor_home(); - //orState is subtracted from future rotor state inputs to align rotor and motor states - + // register ISRs I1.rise(&photointerrupter_isr); I2.rise(&photointerrupter_isr); I3.rise(&photointerrupter_isr); @@ -314,25 +135,24 @@ I3.fall(&photointerrupter_isr); + // 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 + // 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}; + 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]; @@ -340,18 +160,264 @@ Ticker hashcounter; hashcounter.attach(&do_hashcount, 1.0); - //Poll the rotor state and set the motor outputs accordingly to spin the motor + // Poll the rotor state and set the motor outputs accordingly to spin the motor while (1) { - - *key = newKey; - + // compute new hash + *key = new_key; sha256.computeHash(hash, sequence, 64); - if (hash[0] == 0 && hash[1] == 0) { - //putMessage(MSG_NONCE_OK, *nonce); - } + if (hash[0] == 0 && hash[1] == 0) + put_message(MSG_NONCE_OK, *nonce); (*nonce)++; hashcount++; } } + +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 comms_out_fn() +{ + while(1) { + 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); + } +} + +// 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); +} + +// 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 + + 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; +} + +// motor control thread sets a timer ISR, this is the handler +void motor_ctrl_timer_isr() { motor_ctrl_thrd.signal_set(0x1); } + +// 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; + + 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(); + + 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; + if (!count) { + put_message(MSG_MAX_SPD, target_speed); + } + // 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(); + } +} + +// parse input with sscanf() +void parse_serial_in(char *s) +{ + // 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(); + if(target_speed_ == 0) target_speed_ = 0; + 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); +} + +void comms_in_fn() +{ + // register serial interrupt handler + pc.attach(&serial_isr); + + 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 (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; + } + } +} + +// 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 motor_out(int8_t driveState, uint32_t t){ + + // Lookup the output byte from the drive state. + int8_t driveOut = drive_table[driveState & 0x07]; + + // Turn off first + if (~driveOut & 0x01) L1L.pulsewidth_us(0); + if (~driveOut & 0x02) L1H = 1; + if (~driveOut & 0x04) L2L.pulsewidth_us(0); + if (~driveOut & 0x08) L2H = 1; + if (~driveOut & 0x10) L3L.pulsewidth_us(0); + if (~driveOut & 0x20) L3H = 1; + + // Then turn on + if (driveOut & 0x01) L1L.pulsewidth_us(t); + if (driveOut & 0x02) L1H = 0; + if (driveOut & 0x04) L2L.pulsewidth_us(t); + if (driveOut & 0x08) L2H = 0; + if (driveOut & 0x10) L3L.pulsewidth_us(t); + if (driveOut & 0x20) L3H = 0; +} + +// 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 motor_home() +{ + // Put the motor in drive state 0 and wait for it to stabilise + motor_out(0, MAX_TORQUE); + wait(2.0); + + // Get the rotor state + return read_rotor_state(); +}