![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
First Commit
Dependencies: mbed Crypto_light mbed-rtos
Spin it 2 win it
main.cpp
- Committer:
- TrebleStick
- Date:
- 2018-03-21
- Revision:
- 26:1a7a154e375b
- Parent:
- 25:bf5fff055aef
File content as of revision 26:1a7a154e375b:
#include "mbed.h" #include "Crypto_light/hash/SHA256.h" #include "mbed-rtos/rtos/rtos.h" // Photointerrupter input pins #define I1pin D2 #define I2pin D11 #define I3pin D12 // Incremental encoder input pins #define CHA D7 #define CHB D8 // Motor Drive output pins //Mask in output byte #define L1Lpin D4 //0x01 #define L1Hpin D5 //0x02 #define L2Lpin D3 //0x04 #define L2Hpin D6 //0x08 #define L3Lpin D9 //0x10 #define L3Hpin D10 //0x20 // max input length #define CHAR_ARR_SIZE 18 // pwm/motor control definitions #define MAX_PWM_PERIOD 2000 #define MAX_TORQUE 1000 #define KP 20 #define KD 20 // 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)) 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}; // Instantiate the serial port RawSerial pc(SERIAL_TX, SERIAL_RX); // Status LED DigitalOut led1(LED1); // Photointerrupter inputs InterruptIn I1(I1pin); InterruptIn I2(I2pin); InterruptIn I3(I3pin); // motor drive outputs PwmOut L1L(L1Lpin); PwmOut L2L(L2Lpin); PwmOut L3L(L3Lpin); DigitalOut L1H(L1Hpin); DigitalOut L2H(L2Hpin); 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; 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); 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); // 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++; } } 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(); }