First Commit
Dependencies: mbed Crypto_light mbed-rtos
Spin it 2 win it
main.cpp@26:1a7a154e375b, 2018-03-21 (annotated)
- Committer:
- TrebleStick
- Date:
- Wed Mar 21 12:34:39 2018 +0000
- Revision:
- 26:1a7a154e375b
- Parent:
- 25:bf5fff055aef
Final;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TrebleStick | 0:88c3d6c8a4eb | 1 | #include "mbed.h" |
andrebharath | 3:2e32d7974962 | 2 | #include "Crypto_light/hash/SHA256.h" |
andrebharath | 3:2e32d7974962 | 3 | #include "mbed-rtos/rtos/rtos.h" |
TrebleStick | 0:88c3d6c8a4eb | 4 | |
TrebleStick | 26:1a7a154e375b | 5 | // Photointerrupter input pins |
TrebleStick | 0:88c3d6c8a4eb | 6 | #define I1pin D2 |
TrebleStick | 0:88c3d6c8a4eb | 7 | #define I2pin D11 |
TrebleStick | 0:88c3d6c8a4eb | 8 | #define I3pin D12 |
andrebharath | 9:ecef1e8cbe3d | 9 | |
TrebleStick | 26:1a7a154e375b | 10 | // Incremental encoder input pins |
TrebleStick | 0:88c3d6c8a4eb | 11 | #define CHA D7 |
TrebleStick | 12:1b2e2540e4e1 | 12 | #define CHB D8 |
andrebharath | 9:ecef1e8cbe3d | 13 | |
TrebleStick | 26:1a7a154e375b | 14 | // Motor Drive output pins //Mask in output byte |
TrebleStick | 0:88c3d6c8a4eb | 15 | #define L1Lpin D4 //0x01 |
TrebleStick | 0:88c3d6c8a4eb | 16 | #define L1Hpin D5 //0x02 |
TrebleStick | 0:88c3d6c8a4eb | 17 | #define L2Lpin D3 //0x04 |
TrebleStick | 0:88c3d6c8a4eb | 18 | #define L2Hpin D6 //0x08 |
TrebleStick | 0:88c3d6c8a4eb | 19 | #define L3Lpin D9 //0x10 |
TrebleStick | 0:88c3d6c8a4eb | 20 | #define L3Hpin D10 //0x20 |
TrebleStick | 0:88c3d6c8a4eb | 21 | |
TrebleStick | 26:1a7a154e375b | 22 | // max input length |
TrebleStick | 26:1a7a154e375b | 23 | #define CHAR_ARR_SIZE 18 |
TrebleStick | 26:1a7a154e375b | 24 | |
TrebleStick | 26:1a7a154e375b | 25 | // pwm/motor control definitions |
andrebharath | 17:80159ace5ddf | 26 | #define MAX_PWM_PERIOD 2000 |
andrebharath | 18:05e5d280a082 | 27 | #define MAX_TORQUE 1000 |
TrebleStick | 26:1a7a154e375b | 28 | #define KP 20 |
TrebleStick | 26:1a7a154e375b | 29 | #define KD 20 |
andrebharath | 3:2e32d7974962 | 30 | |
TrebleStick | 26:1a7a154e375b | 31 | // function-like macros for utility |
TrebleStick | 26:1a7a154e375b | 32 | #define sgn(x) ((x)/abs(x)) |
TrebleStick | 26:1a7a154e375b | 33 | #define max(x,y) ((x)>=(y)?(x):(y)) |
TrebleStick | 26:1a7a154e375b | 34 | #define min(x,y) ((x)>=(y)?(y):(x)) |
andrebharath | 9:ecef1e8cbe3d | 35 | |
TrebleStick | 26:1a7a154e375b | 36 | enum MSG {MSG_RESET, MSG_HASHCOUNT, MSG_NONCE_OK, MSG_OVERFLOW, MSG_ROT_PEN, |
TrebleStick | 26:1a7a154e375b | 37 | MSG_MAX_SPD, MSG_NEW_KEY, MSG_INP_ERR, MSG_TORQUE, MSG_TEST, |
TrebleStick | 26:1a7a154e375b | 38 | MSG_CUR_SPD, MSG_POS, MSG_NEW_VEL, MSG_NEW_ROTOR_POS}; |
andrebharath | 17:80159ace5ddf | 39 | |
andrebharath | 9:ecef1e8cbe3d | 40 | |
TrebleStick | 26:1a7a154e375b | 41 | // Instantiate the serial port |
andrebharath | 9:ecef1e8cbe3d | 42 | RawSerial pc(SERIAL_TX, SERIAL_RX); |
andrebharath | 9:ecef1e8cbe3d | 43 | |
TrebleStick | 26:1a7a154e375b | 44 | // Status LED |
andrebharath | 9:ecef1e8cbe3d | 45 | DigitalOut led1(LED1); |
andrebharath | 9:ecef1e8cbe3d | 46 | |
TrebleStick | 26:1a7a154e375b | 47 | // Photointerrupter inputs |
andrebharath | 9:ecef1e8cbe3d | 48 | InterruptIn I1(I1pin); |
andrebharath | 9:ecef1e8cbe3d | 49 | InterruptIn I2(I2pin); |
andrebharath | 9:ecef1e8cbe3d | 50 | InterruptIn I3(I3pin); |
andrebharath | 9:ecef1e8cbe3d | 51 | |
TrebleStick | 26:1a7a154e375b | 52 | // motor drive outputs |
andrebharath | 17:80159ace5ddf | 53 | PwmOut L1L(L1Lpin); |
TrebleStick | 26:1a7a154e375b | 54 | PwmOut L2L(L2Lpin); |
TrebleStick | 26:1a7a154e375b | 55 | PwmOut L3L(L3Lpin); |
andrebharath | 9:ecef1e8cbe3d | 56 | DigitalOut L1H(L1Hpin); |
andrebharath | 9:ecef1e8cbe3d | 57 | DigitalOut L2H(L2Hpin); |
andrebharath | 9:ecef1e8cbe3d | 58 | DigitalOut L3H(L3Hpin); |
andrebharath | 9:ecef1e8cbe3d | 59 | |
TrebleStick | 26:1a7a154e375b | 60 | // givens from coursework handouts - motor states etc |
TrebleStick | 26:1a7a154e375b | 61 | const int8_t drive_table[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00}; |
TrebleStick | 26:1a7a154e375b | 62 | const int8_t state_map[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07}; |
TrebleStick | 26:1a7a154e375b | 63 | volatile int8_t lead = 2, // phase lead, -2 for backwards, 2 for forwards |
TrebleStick | 26:1a7a154e375b | 64 | origin_state = 0; |
andrebharath | 9:ecef1e8cbe3d | 65 | |
andrebharath | 9:ecef1e8cbe3d | 66 | |
TrebleStick | 26:1a7a154e375b | 67 | // threads for serial I/O and motor control |
TrebleStick | 26:1a7a154e375b | 68 | Thread comms_out_thrd(osPriorityNormal, 1024); |
TrebleStick | 26:1a7a154e375b | 69 | Thread comms_in_thrd(osPriorityNormal, 1024); |
TrebleStick | 26:1a7a154e375b | 70 | Thread motor_ctrl_thrd(osPriorityNormal, 2048); |
TrebleStick | 20:a435105305fe | 71 | |
andrebharath | 9:ecef1e8cbe3d | 72 | |
TrebleStick | 26:1a7a154e375b | 73 | // IPC via Mail object; we instantiate here |
TrebleStick | 26:1a7a154e375b | 74 | typedef struct { |
TrebleStick | 26:1a7a154e375b | 75 | char *stub; |
TrebleStick | 26:1a7a154e375b | 76 | uint8_t code; |
TrebleStick | 26:1a7a154e375b | 77 | int32_t data; |
TrebleStick | 26:1a7a154e375b | 78 | } message_t; |
TrebleStick | 26:1a7a154e375b | 79 | |
TrebleStick | 26:1a7a154e375b | 80 | Mail<message_t, 16> msg_out_queue; |
TrebleStick | 12:1b2e2540e4e1 | 81 | |
TrebleStick | 26:1a7a154e375b | 82 | // mutex variables |
TrebleStick | 26:1a7a154e375b | 83 | Mutex new_key_mutex; |
TrebleStick | 26:1a7a154e375b | 84 | Mutex target_speed_mutex; |
TrebleStick | 26:1a7a154e375b | 85 | Mutex rotations_pending_mutex; |
TrebleStick | 12:1b2e2540e4e1 | 86 | |
TrebleStick | 26:1a7a154e375b | 87 | // instantiate a queue to buffer incoming characters |
TrebleStick | 26:1a7a154e375b | 88 | Queue<void, 8> serial_in_queue; |
TrebleStick | 26:1a7a154e375b | 89 | // motor control global variables |
TrebleStick | 26:1a7a154e375b | 90 | volatile int32_t motor_position = 0, |
TrebleStick | 26:1a7a154e375b | 91 | target_speed = 256, |
TrebleStick | 26:1a7a154e375b | 92 | torque = 1000; |
TrebleStick | 26:1a7a154e375b | 93 | volatile float rotations_pending = 0; |
TrebleStick | 12:1b2e2540e4e1 | 94 | |
TrebleStick | 26:1a7a154e375b | 95 | // hash count, reset every second when printed |
TrebleStick | 26:1a7a154e375b | 96 | volatile uint16_t hashcount = 0; |
TrebleStick | 26:1a7a154e375b | 97 | // used when selecting a new hash key |
TrebleStick | 26:1a7a154e375b | 98 | volatile uint64_t new_key = 0; |
TrebleStick | 12:1b2e2540e4e1 | 99 | |
TrebleStick | 26:1a7a154e375b | 100 | // logging function & shim macro for stringifying enum |
TrebleStick | 26:1a7a154e375b | 101 | #define put_message(code, data) put_message_(#code, code, data) |
TrebleStick | 26:1a7a154e375b | 102 | void put_message_(char *, uint8_t, int32_t); |
TrebleStick | 12:1b2e2540e4e1 | 103 | |
TrebleStick | 26:1a7a154e375b | 104 | void comms_out_fn(void); // serial output thread main |
TrebleStick | 26:1a7a154e375b | 105 | void comms_in_fn(void); // serial input thread main |
TrebleStick | 26:1a7a154e375b | 106 | void serial_isr(void); // serial event ISR |
TrebleStick | 26:1a7a154e375b | 107 | void photointerrupter_isr(void); // motor state change ISR |
TrebleStick | 26:1a7a154e375b | 108 | void motor_ctrl_fn(void); // motor control thread main |
TrebleStick | 26:1a7a154e375b | 109 | void motor_ctrl_timer_isr(void); // poke motor_ctrl at 100ms intervals |
TrebleStick | 26:1a7a154e375b | 110 | void parse_serial_in(char *); // interpret serial command |
TrebleStick | 26:1a7a154e375b | 111 | void do_hashcount(void); // print current hash count |
TrebleStick | 26:1a7a154e375b | 112 | inline int8_t read_rotor_state(void); // get rotor position |
TrebleStick | 26:1a7a154e375b | 113 | int8_t motor_home(void); // establish motor origin position |
TrebleStick | 26:1a7a154e375b | 114 | void motor_out(int8_t, uint32_t); // do motor output |
TrebleStick | 12:1b2e2540e4e1 | 115 | |
TrebleStick | 26:1a7a154e375b | 116 | int main(void) |
TrebleStick | 26:1a7a154e375b | 117 | { |
andrebharath | 9:ecef1e8cbe3d | 118 | |
TrebleStick | 26:1a7a154e375b | 119 | comms_out_thrd.start(&comms_out_fn); |
TrebleStick | 26:1a7a154e375b | 120 | motor_ctrl_thrd.start(&motor_ctrl_fn); |
TrebleStick | 26:1a7a154e375b | 121 | comms_in_thrd.start(&comms_in_fn); |
andrebharath | 9:ecef1e8cbe3d | 122 | |
TrebleStick | 26:1a7a154e375b | 123 | put_message(MSG_RESET, 0); |
TrebleStick | 12:1b2e2540e4e1 | 124 | |
TrebleStick | 26:1a7a154e375b | 125 | // sync motor to home |
TrebleStick | 26:1a7a154e375b | 126 | rotations_pending = origin_state = motor_home(); |
TrebleStick | 20:a435105305fe | 127 | |
TrebleStick | 26:1a7a154e375b | 128 | // register ISRs |
andrebharath | 3:2e32d7974962 | 129 | I1.rise(&photointerrupter_isr); |
andrebharath | 3:2e32d7974962 | 130 | I2.rise(&photointerrupter_isr); |
andrebharath | 3:2e32d7974962 | 131 | I3.rise(&photointerrupter_isr); |
TrebleStick | 12:1b2e2540e4e1 | 132 | |
andrebharath | 3:2e32d7974962 | 133 | I1.fall(&photointerrupter_isr); |
andrebharath | 3:2e32d7974962 | 134 | I2.fall(&photointerrupter_isr); |
andrebharath | 3:2e32d7974962 | 135 | I3.fall(&photointerrupter_isr); |
TrebleStick | 12:1b2e2540e4e1 | 136 | |
andrebharath | 18:05e5d280a082 | 137 | |
TrebleStick | 26:1a7a154e375b | 138 | // set PWM period |
andrebharath | 18:05e5d280a082 | 139 | L1L.period_us(MAX_PWM_PERIOD); |
andrebharath | 18:05e5d280a082 | 140 | L2L.period_us(MAX_PWM_PERIOD); |
andrebharath | 18:05e5d280a082 | 141 | L3L.period_us(MAX_PWM_PERIOD); |
andrebharath | 18:05e5d280a082 | 142 | |
TrebleStick | 26:1a7a154e375b | 143 | // Calling the ISR once starts the motor movement |
andrebharath | 9:ecef1e8cbe3d | 144 | photointerrupter_isr(); |
TrebleStick | 19:526fd700e1b3 | 145 | |
TrebleStick | 26:1a7a154e375b | 146 | // SHA256-related data |
andrebharath | 9:ecef1e8cbe3d | 147 | SHA256 sha256; |
andrebharath | 3:2e32d7974962 | 148 | uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64, |
TrebleStick | 26:1a7a154e375b | 149 | 0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73, |
TrebleStick | 26:1a7a154e375b | 150 | 0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E, |
TrebleStick | 26:1a7a154e375b | 151 | 0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20, |
TrebleStick | 26:1a7a154e375b | 152 | 0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20, |
TrebleStick | 26:1a7a154e375b | 153 | 0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20, |
TrebleStick | 26:1a7a154e375b | 154 | 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, |
TrebleStick | 26:1a7a154e375b | 155 | 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; |
andrebharath | 3:2e32d7974962 | 156 | uint64_t* key = (uint64_t*)((int)sequence + 48); |
andrebharath | 3:2e32d7974962 | 157 | uint64_t* nonce = (uint64_t*)((int)sequence + 56); |
andrebharath | 3:2e32d7974962 | 158 | uint8_t hash[32]; |
TrebleStick | 12:1b2e2540e4e1 | 159 | |
andrebharath | 9:ecef1e8cbe3d | 160 | Ticker hashcounter; |
andrebharath | 9:ecef1e8cbe3d | 161 | hashcounter.attach(&do_hashcount, 1.0); |
TrebleStick | 12:1b2e2540e4e1 | 162 | |
TrebleStick | 26:1a7a154e375b | 163 | // Poll the rotor state and set the motor outputs accordingly to spin the motor |
TrebleStick | 0:88c3d6c8a4eb | 164 | while (1) { |
TrebleStick | 26:1a7a154e375b | 165 | // compute new hash |
TrebleStick | 26:1a7a154e375b | 166 | *key = new_key; |
andrebharath | 9:ecef1e8cbe3d | 167 | sha256.computeHash(hash, sequence, 64); |
TrebleStick | 12:1b2e2540e4e1 | 168 | |
TrebleStick | 26:1a7a154e375b | 169 | if (hash[0] == 0 && hash[1] == 0) |
TrebleStick | 26:1a7a154e375b | 170 | put_message(MSG_NONCE_OK, *nonce); |
andrebharath | 3:2e32d7974962 | 171 | |
andrebharath | 3:2e32d7974962 | 172 | (*nonce)++; |
andrebharath | 3:2e32d7974962 | 173 | hashcount++; |
TrebleStick | 0:88c3d6c8a4eb | 174 | } |
TrebleStick | 0:88c3d6c8a4eb | 175 | } |
TrebleStick | 26:1a7a154e375b | 176 | |
TrebleStick | 26:1a7a154e375b | 177 | void put_message_(char *str, uint8_t code, int32_t data) |
TrebleStick | 26:1a7a154e375b | 178 | { |
TrebleStick | 26:1a7a154e375b | 179 | message_t *message = msg_out_queue.alloc(); |
TrebleStick | 26:1a7a154e375b | 180 | message->code = code; |
TrebleStick | 26:1a7a154e375b | 181 | message->data = data; |
TrebleStick | 26:1a7a154e375b | 182 | message->stub = str; |
TrebleStick | 26:1a7a154e375b | 183 | msg_out_queue.put(message); |
TrebleStick | 26:1a7a154e375b | 184 | } |
TrebleStick | 26:1a7a154e375b | 185 | |
TrebleStick | 26:1a7a154e375b | 186 | void comms_out_fn() |
TrebleStick | 26:1a7a154e375b | 187 | { |
TrebleStick | 26:1a7a154e375b | 188 | while(1) { |
TrebleStick | 26:1a7a154e375b | 189 | osEvent new_event = msg_out_queue.get(); |
TrebleStick | 26:1a7a154e375b | 190 | message_t *message = (message_t*) new_event.value.p; |
TrebleStick | 26:1a7a154e375b | 191 | pc.printf("[%16s], data: %010d\r\n", |
TrebleStick | 26:1a7a154e375b | 192 | message->stub, |
TrebleStick | 26:1a7a154e375b | 193 | message->data); |
TrebleStick | 26:1a7a154e375b | 194 | msg_out_queue.free(message); |
TrebleStick | 26:1a7a154e375b | 195 | } |
TrebleStick | 26:1a7a154e375b | 196 | } |
TrebleStick | 26:1a7a154e375b | 197 | |
TrebleStick | 26:1a7a154e375b | 198 | // serial port ISR to receive each incoming byte and place into queue |
TrebleStick | 26:1a7a154e375b | 199 | void serial_isr() |
TrebleStick | 26:1a7a154e375b | 200 | { |
TrebleStick | 26:1a7a154e375b | 201 | uint8_t new_char = pc.getc(); |
TrebleStick | 26:1a7a154e375b | 202 | serial_in_queue.put((void*) new_char); |
TrebleStick | 26:1a7a154e375b | 203 | } |
TrebleStick | 26:1a7a154e375b | 204 | |
TrebleStick | 26:1a7a154e375b | 205 | // photointerrupter ISR drives the motors |
TrebleStick | 26:1a7a154e375b | 206 | void photointerrupter_isr() |
TrebleStick | 26:1a7a154e375b | 207 | { |
TrebleStick | 26:1a7a154e375b | 208 | static int8_t old_rotor_state = 0; |
TrebleStick | 26:1a7a154e375b | 209 | int8_t rotor_state = read_rotor_state(); |
TrebleStick | 26:1a7a154e375b | 210 | |
TrebleStick | 26:1a7a154e375b | 211 | motor_out((rotor_state-origin_state+lead+6)%6, torque); //+6 to make sure the remainder is positive |
TrebleStick | 26:1a7a154e375b | 212 | |
TrebleStick | 26:1a7a154e375b | 213 | if (rotor_state - old_rotor_state == 5) |
TrebleStick | 26:1a7a154e375b | 214 | motor_position--; |
TrebleStick | 26:1a7a154e375b | 215 | else if (rotor_state - old_rotor_state == -5) |
TrebleStick | 26:1a7a154e375b | 216 | motor_position++; |
TrebleStick | 26:1a7a154e375b | 217 | else |
TrebleStick | 26:1a7a154e375b | 218 | motor_position += (rotor_state - old_rotor_state); |
TrebleStick | 26:1a7a154e375b | 219 | |
TrebleStick | 26:1a7a154e375b | 220 | old_rotor_state = rotor_state; |
TrebleStick | 26:1a7a154e375b | 221 | } |
TrebleStick | 26:1a7a154e375b | 222 | |
TrebleStick | 26:1a7a154e375b | 223 | // motor control thread sets a timer ISR, this is the handler |
TrebleStick | 26:1a7a154e375b | 224 | void motor_ctrl_timer_isr() { motor_ctrl_thrd.signal_set(0x1); } |
TrebleStick | 26:1a7a154e375b | 225 | |
TrebleStick | 26:1a7a154e375b | 226 | // motor control thread main |
TrebleStick | 26:1a7a154e375b | 227 | void motor_ctrl_fn() |
TrebleStick | 26:1a7a154e375b | 228 | { |
TrebleStick | 26:1a7a154e375b | 229 | Ticker motor_control_ticker; |
TrebleStick | 26:1a7a154e375b | 230 | Timer timer; |
TrebleStick | 26:1a7a154e375b | 231 | |
TrebleStick | 26:1a7a154e375b | 232 | uint8_t count = 0; |
TrebleStick | 26:1a7a154e375b | 233 | |
TrebleStick | 26:1a7a154e375b | 234 | int32_t cur_pos = 0, |
TrebleStick | 26:1a7a154e375b | 235 | old_pos = 0, |
TrebleStick | 26:1a7a154e375b | 236 | cur_speed, |
TrebleStick | 26:1a7a154e375b | 237 | ys, |
TrebleStick | 26:1a7a154e375b | 238 | yr; |
TrebleStick | 26:1a7a154e375b | 239 | |
TrebleStick | 26:1a7a154e375b | 240 | uint32_t cur_time = 0, |
TrebleStick | 26:1a7a154e375b | 241 | old_time = 0, |
TrebleStick | 26:1a7a154e375b | 242 | time_diff; |
TrebleStick | 26:1a7a154e375b | 243 | |
TrebleStick | 26:1a7a154e375b | 244 | float cur_err = 0.0f, |
TrebleStick | 26:1a7a154e375b | 245 | old_err = 0.0f, |
TrebleStick | 26:1a7a154e375b | 246 | err_diff; |
TrebleStick | 26:1a7a154e375b | 247 | |
TrebleStick | 26:1a7a154e375b | 248 | motor_control_ticker.attach_us(&motor_ctrl_timer_isr,100000); |
TrebleStick | 26:1a7a154e375b | 249 | |
TrebleStick | 26:1a7a154e375b | 250 | timer.start(); |
TrebleStick | 26:1a7a154e375b | 251 | |
TrebleStick | 26:1a7a154e375b | 252 | while(1) { |
TrebleStick | 26:1a7a154e375b | 253 | // wait for the 100ms boundary |
TrebleStick | 26:1a7a154e375b | 254 | motor_ctrl_thrd.signal_wait(0x1); |
TrebleStick | 26:1a7a154e375b | 255 | |
TrebleStick | 26:1a7a154e375b | 256 | // read state & timestamp |
TrebleStick | 26:1a7a154e375b | 257 | cur_time = timer.read(); |
TrebleStick | 26:1a7a154e375b | 258 | cur_pos = motor_position; |
TrebleStick | 26:1a7a154e375b | 259 | |
TrebleStick | 26:1a7a154e375b | 260 | // compute speed |
TrebleStick | 26:1a7a154e375b | 261 | time_diff = cur_time - old_time; |
TrebleStick | 26:1a7a154e375b | 262 | cur_speed = (cur_pos - old_pos) / time_diff; |
TrebleStick | 26:1a7a154e375b | 263 | |
TrebleStick | 26:1a7a154e375b | 264 | // prep values for next time through loop |
TrebleStick | 26:1a7a154e375b | 265 | old_time = cur_time; |
TrebleStick | 26:1a7a154e375b | 266 | old_pos = cur_pos; |
TrebleStick | 26:1a7a154e375b | 267 | |
TrebleStick | 26:1a7a154e375b | 268 | count = ++count % 10; |
TrebleStick | 26:1a7a154e375b | 269 | if (!count) { |
TrebleStick | 26:1a7a154e375b | 270 | put_message(MSG_MAX_SPD, target_speed); |
TrebleStick | 26:1a7a154e375b | 271 | } |
TrebleStick | 26:1a7a154e375b | 272 | // update with motor status |
TrebleStick | 26:1a7a154e375b | 273 | /* |
TrebleStick | 26:1a7a154e375b | 274 | * if (!count) { |
TrebleStick | 26:1a7a154e375b | 275 | * put_message(MSG_CUR_SPD, cur_speed); |
TrebleStick | 26:1a7a154e375b | 276 | * put_message(MSG_MAX_SPD, target_speed); |
TrebleStick | 26:1a7a154e375b | 277 | * put_message(MSG_POS, (cur_pos/6)); |
TrebleStick | 26:1a7a154e375b | 278 | * put_message(MSG_ROT_PEN, rotations_pending); |
TrebleStick | 26:1a7a154e375b | 279 | * put_message(MSG_TORQUE, torque); |
TrebleStick | 26:1a7a154e375b | 280 | * } |
TrebleStick | 26:1a7a154e375b | 281 | */ |
TrebleStick | 26:1a7a154e375b | 282 | |
TrebleStick | 26:1a7a154e375b | 283 | // compute position error |
TrebleStick | 26:1a7a154e375b | 284 | cur_err = rotations_pending - (cur_pos/6.0f); |
TrebleStick | 26:1a7a154e375b | 285 | err_diff = cur_err - old_err; |
TrebleStick | 26:1a7a154e375b | 286 | old_err = cur_err; |
TrebleStick | 26:1a7a154e375b | 287 | |
TrebleStick | 26:1a7a154e375b | 288 | // compute torques |
TrebleStick | 26:1a7a154e375b | 289 | ys = (int32_t) (20 * (target_speed - abs(cur_speed))) * sgn(cur_err); |
TrebleStick | 26:1a7a154e375b | 290 | yr = (int32_t) ((20 * cur_err) + (40 * err_diff)); |
TrebleStick | 26:1a7a154e375b | 291 | |
TrebleStick | 26:1a7a154e375b | 292 | // select minimum absolute value torque |
TrebleStick | 26:1a7a154e375b | 293 | if (cur_speed < 0) |
TrebleStick | 26:1a7a154e375b | 294 | torque = max(ys, yr); |
TrebleStick | 26:1a7a154e375b | 295 | else |
TrebleStick | 26:1a7a154e375b | 296 | torque = min(ys, yr); |
TrebleStick | 26:1a7a154e375b | 297 | |
TrebleStick | 26:1a7a154e375b | 298 | // fix torque if negative |
TrebleStick | 26:1a7a154e375b | 299 | if (torque < 0) |
TrebleStick | 26:1a7a154e375b | 300 | torque = -torque, // <- comma operator in action |
TrebleStick | 26:1a7a154e375b | 301 | lead = -2; |
TrebleStick | 26:1a7a154e375b | 302 | else |
TrebleStick | 26:1a7a154e375b | 303 | lead = 2; |
TrebleStick | 26:1a7a154e375b | 304 | |
TrebleStick | 26:1a7a154e375b | 305 | // cap torque |
TrebleStick | 26:1a7a154e375b | 306 | if (torque > MAX_TORQUE) |
TrebleStick | 26:1a7a154e375b | 307 | torque = MAX_TORQUE; |
TrebleStick | 26:1a7a154e375b | 308 | |
TrebleStick | 26:1a7a154e375b | 309 | // finally, give the motor a kick |
TrebleStick | 26:1a7a154e375b | 310 | photointerrupter_isr(); |
TrebleStick | 26:1a7a154e375b | 311 | } |
TrebleStick | 26:1a7a154e375b | 312 | } |
TrebleStick | 26:1a7a154e375b | 313 | |
TrebleStick | 26:1a7a154e375b | 314 | // parse input with sscanf() |
TrebleStick | 26:1a7a154e375b | 315 | void parse_serial_in(char *s) |
TrebleStick | 26:1a7a154e375b | 316 | { |
TrebleStick | 26:1a7a154e375b | 317 | // shadow output variables so writes are guaranteed atomic |
TrebleStick | 26:1a7a154e375b | 318 | uint64_t new_key_; |
TrebleStick | 26:1a7a154e375b | 319 | int32_t torque_; |
TrebleStick | 26:1a7a154e375b | 320 | int32_t target_speed_; |
TrebleStick | 26:1a7a154e375b | 321 | float rotations_pending_; |
TrebleStick | 26:1a7a154e375b | 322 | |
TrebleStick | 26:1a7a154e375b | 323 | if (sscanf(s, "R%f", &rotations_pending_)) { |
TrebleStick | 26:1a7a154e375b | 324 | |
TrebleStick | 26:1a7a154e375b | 325 | rotations_pending += rotations_pending_; |
TrebleStick | 26:1a7a154e375b | 326 | put_message(MSG_ROT_PEN, rotations_pending); |
TrebleStick | 26:1a7a154e375b | 327 | |
TrebleStick | 26:1a7a154e375b | 328 | } else if (sscanf(s, "V%d", &target_speed_)) { |
TrebleStick | 26:1a7a154e375b | 329 | |
TrebleStick | 26:1a7a154e375b | 330 | target_speed_mutex.lock(); |
TrebleStick | 26:1a7a154e375b | 331 | if(target_speed_ == 0) target_speed_ = 0; |
TrebleStick | 26:1a7a154e375b | 332 | target_speed = target_speed_; |
TrebleStick | 26:1a7a154e375b | 333 | target_speed_mutex.unlock(); |
TrebleStick | 26:1a7a154e375b | 334 | put_message(MSG_NEW_VEL, target_speed); |
TrebleStick | 26:1a7a154e375b | 335 | |
TrebleStick | 26:1a7a154e375b | 336 | } else if (sscanf(s, "K%llx", &new_key_)) { |
TrebleStick | 26:1a7a154e375b | 337 | |
TrebleStick | 26:1a7a154e375b | 338 | new_key_mutex.lock(); |
TrebleStick | 26:1a7a154e375b | 339 | new_key = new_key_; |
TrebleStick | 26:1a7a154e375b | 340 | new_key_mutex.unlock(); |
TrebleStick | 26:1a7a154e375b | 341 | put_message(MSG_NEW_KEY, new_key); |
TrebleStick | 26:1a7a154e375b | 342 | |
TrebleStick | 26:1a7a154e375b | 343 | } else if (sscanf(s, "T%u", &torque_)) { |
TrebleStick | 26:1a7a154e375b | 344 | torque = torque_; |
TrebleStick | 26:1a7a154e375b | 345 | photointerrupter_isr(); //Give it a kick |
TrebleStick | 26:1a7a154e375b | 346 | // put_message(MSG_TORQUE, torque); |
TrebleStick | 26:1a7a154e375b | 347 | } else |
TrebleStick | 26:1a7a154e375b | 348 | put_message(MSG_INP_ERR, 0x404); |
TrebleStick | 26:1a7a154e375b | 349 | } |
TrebleStick | 26:1a7a154e375b | 350 | |
TrebleStick | 26:1a7a154e375b | 351 | void comms_in_fn() |
TrebleStick | 26:1a7a154e375b | 352 | { |
TrebleStick | 26:1a7a154e375b | 353 | // register serial interrupt handler |
TrebleStick | 26:1a7a154e375b | 354 | pc.attach(&serial_isr); |
TrebleStick | 26:1a7a154e375b | 355 | |
TrebleStick | 26:1a7a154e375b | 356 | char char_seq[CHAR_ARR_SIZE] = ""; |
TrebleStick | 26:1a7a154e375b | 357 | uint8_t buf_pos = 0; |
TrebleStick | 26:1a7a154e375b | 358 | |
TrebleStick | 26:1a7a154e375b | 359 | while (1) { |
TrebleStick | 26:1a7a154e375b | 360 | if (buf_pos >= CHAR_ARR_SIZE) { |
TrebleStick | 26:1a7a154e375b | 361 | put_message(MSG_OVERFLOW, buf_pos); |
TrebleStick | 26:1a7a154e375b | 362 | buf_pos = 0; |
TrebleStick | 26:1a7a154e375b | 363 | } else { |
TrebleStick | 26:1a7a154e375b | 364 | osEvent new_event = serial_in_queue.get(); |
TrebleStick | 26:1a7a154e375b | 365 | uint8_t new_char = (uint8_t)new_event.value.p; |
TrebleStick | 26:1a7a154e375b | 366 | |
TrebleStick | 26:1a7a154e375b | 367 | if (new_char == '\r' || new_char == '\n') { |
TrebleStick | 26:1a7a154e375b | 368 | char_seq[buf_pos] = '\0'; |
TrebleStick | 26:1a7a154e375b | 369 | buf_pos = 0; |
TrebleStick | 26:1a7a154e375b | 370 | parse_serial_in(char_seq); |
TrebleStick | 26:1a7a154e375b | 371 | } else |
TrebleStick | 26:1a7a154e375b | 372 | char_seq[buf_pos++] = new_char; |
TrebleStick | 26:1a7a154e375b | 373 | } |
TrebleStick | 26:1a7a154e375b | 374 | } |
TrebleStick | 26:1a7a154e375b | 375 | } |
TrebleStick | 26:1a7a154e375b | 376 | |
TrebleStick | 26:1a7a154e375b | 377 | // timer ISR to print/reset hash counts every second |
TrebleStick | 26:1a7a154e375b | 378 | void do_hashcount() |
TrebleStick | 26:1a7a154e375b | 379 | { |
TrebleStick | 26:1a7a154e375b | 380 | put_message(MSG_HASHCOUNT, hashcount); |
TrebleStick | 26:1a7a154e375b | 381 | hashcount = 0; |
TrebleStick | 26:1a7a154e375b | 382 | } |
TrebleStick | 26:1a7a154e375b | 383 | |
TrebleStick | 26:1a7a154e375b | 384 | |
TrebleStick | 26:1a7a154e375b | 385 | //Set a given drive state |
TrebleStick | 26:1a7a154e375b | 386 | void motor_out(int8_t driveState, uint32_t t){ |
TrebleStick | 26:1a7a154e375b | 387 | |
TrebleStick | 26:1a7a154e375b | 388 | // Lookup the output byte from the drive state. |
TrebleStick | 26:1a7a154e375b | 389 | int8_t driveOut = drive_table[driveState & 0x07]; |
TrebleStick | 26:1a7a154e375b | 390 | |
TrebleStick | 26:1a7a154e375b | 391 | // Turn off first |
TrebleStick | 26:1a7a154e375b | 392 | if (~driveOut & 0x01) L1L.pulsewidth_us(0); |
TrebleStick | 26:1a7a154e375b | 393 | if (~driveOut & 0x02) L1H = 1; |
TrebleStick | 26:1a7a154e375b | 394 | if (~driveOut & 0x04) L2L.pulsewidth_us(0); |
TrebleStick | 26:1a7a154e375b | 395 | if (~driveOut & 0x08) L2H = 1; |
TrebleStick | 26:1a7a154e375b | 396 | if (~driveOut & 0x10) L3L.pulsewidth_us(0); |
TrebleStick | 26:1a7a154e375b | 397 | if (~driveOut & 0x20) L3H = 1; |
TrebleStick | 26:1a7a154e375b | 398 | |
TrebleStick | 26:1a7a154e375b | 399 | // Then turn on |
TrebleStick | 26:1a7a154e375b | 400 | if (driveOut & 0x01) L1L.pulsewidth_us(t); |
TrebleStick | 26:1a7a154e375b | 401 | if (driveOut & 0x02) L1H = 0; |
TrebleStick | 26:1a7a154e375b | 402 | if (driveOut & 0x04) L2L.pulsewidth_us(t); |
TrebleStick | 26:1a7a154e375b | 403 | if (driveOut & 0x08) L2H = 0; |
TrebleStick | 26:1a7a154e375b | 404 | if (driveOut & 0x10) L3L.pulsewidth_us(t); |
TrebleStick | 26:1a7a154e375b | 405 | if (driveOut & 0x20) L3H = 0; |
TrebleStick | 26:1a7a154e375b | 406 | } |
TrebleStick | 26:1a7a154e375b | 407 | |
TrebleStick | 26:1a7a154e375b | 408 | // Convert photointerrupter inputs to a rotor state |
TrebleStick | 26:1a7a154e375b | 409 | inline int8_t read_rotor_state() |
TrebleStick | 26:1a7a154e375b | 410 | { |
TrebleStick | 26:1a7a154e375b | 411 | return state_map[I1 + 2*I2 + 4*I3]; |
TrebleStick | 26:1a7a154e375b | 412 | } |
TrebleStick | 26:1a7a154e375b | 413 | |
TrebleStick | 26:1a7a154e375b | 414 | // Basic synchronisation routine |
TrebleStick | 26:1a7a154e375b | 415 | int8_t motor_home() |
TrebleStick | 26:1a7a154e375b | 416 | { |
TrebleStick | 26:1a7a154e375b | 417 | // Put the motor in drive state 0 and wait for it to stabilise |
TrebleStick | 26:1a7a154e375b | 418 | motor_out(0, MAX_TORQUE); |
TrebleStick | 26:1a7a154e375b | 419 | wait(2.0); |
TrebleStick | 26:1a7a154e375b | 420 | |
TrebleStick | 26:1a7a154e375b | 421 | // Get the rotor state |
TrebleStick | 26:1a7a154e375b | 422 | return read_rotor_state(); |
TrebleStick | 26:1a7a154e375b | 423 | } |