SpinnyBoi / Mbed 2 deprecated Spinnybois

First Commit

Dependencies:   mbed Crypto_light mbed-rtos

Spin it 2 win it

main.cpp@29:e28682c4b4cb, 2018-03-23 (annotated)

Committer:
henryeshbaugh
Date:
Fri Mar 23 18:54:18 2018 +0000
Revision:
29:e28682c4b4cb
Parent:
28:8076013fbef5 
oops missed two lines

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