SpinnyBoi / Mbed 2 deprecated Spinnybois

First Commit

Dependencies:   mbed Crypto_light mbed-rtos

Spin it 2 win it

main.cpp@27:d50f1914f23a, 2018-03-23 (annotated)

Committer:
henryeshbaugh
Date:
Fri Mar 23 16:35:17 2018 +0000
Revision:
27:d50f1914f23a
Parent:
25:bf5fff055aef
Child:
28:8076013fbef5
code cleanup; ; andre sucks hank rules

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