SpinnyBoi / Mbed 2 deprecated Spinnybois

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?

UserRevisionLine numberNew 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 }