File content as of revision 29:e28682c4b4cb:

#include "mbed.h"
#include "Crypto_light/hash/SHA256.h"
#include "mbed-rtos/rtos/rtos.h"

/* Photointerruptor input pins */
#define I1pin D2
#define I2pin D11
#define I3pin D12

/* Incremental encoder input pins */
#define CHA   D7
#define CHB   D8

/* Motor Drive output pins  Mask in output byte */
#define L1Lpin D4           /* 0x01 */
#define L1Hpin D5           /* 0x02 */
#define L2Lpin D3           /* 0x04 */
#define L2Hpin D6           /* 0x08 */
#define L3Lpin D9           /* 0x10 */
#define L3Hpin D10          /* 0x20 */

/* max input length */
#define CHAR_ARR_SIZE 18

/* pwm/motor control definitions */
#define MAX_PWM_PERIOD 2000
#define MAX_TORQUE 1000
#define KP 20
#define KD 20

/* function-like macros for utility */
#define sgn(x) ((x)/abs(x))
#define max(x,y) ((x)>=(y)?(x):(y))
#define min(x,y) ((x)>=(y)?(y):(x))

#ifdef __GNUC__
#define likely(x)   __builtin_expect((x), 1)
#define unlikely(x) __builtin_expect((x), 0)
#else
#define likely(x) (x)
#define unlikely(x) (x)
#endif

enum MSG {MSG_RESET,   MSG_HASHCOUNT, MSG_NONCE_OK, MSG_OVERFLOW, MSG_ROT_PEN,
          MSG_MAX_SPD, MSG_NEW_KEY,   MSG_INP_ERR,  MSG_TORQUE,   MSG_TEST,
      MSG_CUR_SPD, MSG_POS,       MSG_NEW_VEL,  MSG_NEW_ROTOR_POS};


/* Instantiate the serial port */
RawSerial pc(SERIAL_TX, SERIAL_RX);

/* Status LED */
DigitalOut led1(LED1);

/* Photointerrupter inputs */
InterruptIn I1(I1pin);
InterruptIn I2(I2pin);
InterruptIn I3(I3pin);

/* motor drive outputs */
PwmOut L1L(L1Lpin);
PwmOut L2L(L2Lpin);
PwmOut L3L(L3Lpin);
DigitalOut L1H(L1Hpin);
DigitalOut L2H(L2Hpin);
DigitalOut L3H(L3Hpin);

/* givens from coursework handouts - motor states etc */
const int8_t    drive_table[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
const int8_t    state_map[]   = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};

volatile int8_t lead = 2,          /* phase lead, -2 for backwards, 2 for forwards */
                origin_state = 0;


/* threads for serial I/O and motor control */
Thread comms_out_thrd(osPriorityLow, 1024);
Thread comms_in_thrd(osPriorityLow, 1024);
Thread motor_ctrl_thrd(osPriorityNormal, 2048);


/* IPC via Mail object; we instantiate here */
typedef struct {
    char *stub;
    uint8_t code;
    int32_t data;
} message_t;

Mail<message_t, 16> msg_out_queue;

/* bools are 8 bits and so access is atomic */
volatile bool key_updated = false, spin_forever = false;

/* instantiate a queue to buffer incoming characters */
Queue<void, 8>    serial_in_queue;
/* motor control global variables */
volatile int32_t  motor_position = 0,
                  target_speed = 256,
          torque = 1000;
volatile float    rotations_pending = 0;

volatile uint16_t hashcount = 0;  /* hash count, reset every second when printed */
volatile uint64_t new_key = 0;    /* used when selecting a new hash key */

/* logging function & shim macro for stringifying enum */
#define put_message(code, data) put_message_(#code, code, data)
void          put_message_(char *, uint8_t, int32_t);
void          comms_out_fn(void);          /* serial output thread main          */
void          comms_in_fn(void);           /* serial input thread main           */
void          serial_isr(void);            /* serial event ISR                   */
void          photointerrupter_isr(void);  /* motor state change ISR             */
void          motor_ctrl_fn(void);         /* motor control thread main          */
void          motor_ctrl_timer_isr(void);  /* poke motor_ctrl at 100ms intervals */
void          parse_serial_in(char *);     /* interpret serial command           */
void          do_hashcount(void);          /* print current hash count           */
inline int8_t read_rotor_state(void);      /* get rotor position                 */
int8_t        motor_home(void);            /* establish motor origin position    */
void          motor_out(int8_t, uint32_t); /* do motor output                    */

int main(void)
{

    comms_out_thrd.start(&comms_out_fn);
    motor_ctrl_thrd.start(&motor_ctrl_fn);
    comms_in_thrd.start(&comms_in_fn);

    put_message(MSG_RESET, 0);

    /* sync motor to home */
    rotations_pending = origin_state = motor_home();

    /* register ISRs */
    I1.rise(&photointerrupter_isr);
    I2.rise(&photointerrupter_isr);
    I3.rise(&photointerrupter_isr);

    I1.fall(&photointerrupter_isr);
    I2.fall(&photointerrupter_isr);
    I3.fall(&photointerrupter_isr);


    /* set PWM period */
    L1L.period_us(MAX_PWM_PERIOD);
    L2L.period_us(MAX_PWM_PERIOD);
    L3L.period_us(MAX_PWM_PERIOD);

    /* Calling the ISR once starts the motor movement */
    photointerrupter_isr();

    /* SHA256-related data */
    SHA256 sha256;
    uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,
                          0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,
                          0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,
                          0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,
                          0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,
                          0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,
                          0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
                          0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};

    uint64_t *key   = (uint64_t*)((int)sequence + 48);
    uint64_t *nonce = (uint64_t*)((int)sequence + 56);
    uint8_t   hash[32];

    Ticker hashcounter;
    hashcounter.attach(&do_hashcount, 1.0);


    while (1) {
        /* compute new hash - let gcc know this is not likely */
        if (unlikely(key_updated)) {
            /* no serialization is needed here; a new serial key  *
             * command needs 18 bytes over serial (15ms) - we     *
             * run at least 8k hashes per second, meaning this    *
             * condition is hit roughly every 125us. Even if two  *
             * key change commands were sent at once, we wouldn't *
             * hit a race condition. The worry is we get          *
             * scheduled out in favor of serial in halfway        *
             * through writing the new key (64 bits) specifically * 
             * to get a new key from a user command, but that     *
             * /can't/ happen because of the serial latency.      */
            *key = new_key;
            key_updated = false;
        }
        /* compute the hash */
        sha256.computeHash(hash, sequence, 64);

        if (hash[0] == 0 && hash[1] == 0)
            put_message(MSG_NONCE_OK, *nonce);

        (*nonce)++;
        hashcount++;
    }
}

void put_message_(char *str, uint8_t code, int32_t data)
{
    message_t *message = msg_out_queue.alloc();
    
    message->code = code;
    message->data = data;
    message->stub = str;

    msg_out_queue.put(message);
}

void comms_out_fn()
{
    while(1) {
        osEvent new_event = msg_out_queue.get();
        message_t *message = (message_t*) new_event.value.p;
        pc.printf("[%d:%16s], data: %010d\r\n",
              message->code,
              message->stub,
              message->data);
        msg_out_queue.free(message);
     }
}

/* serial port ISR to receive each incoming byte and place into queue */
void serial_isr()
{
    uint8_t new_char = pc.getc();
    serial_in_queue.put((void*) new_char);
}

/* photointerrupter ISR drives the motors */
void photointerrupter_isr()
{
    static int8_t old_rotor_state = 0;
    int8_t rotor_state = read_rotor_state();

    motor_out((rotor_state-origin_state+lead+6)%6, torque); /* +6 to make sure the remainder is positive */

    if (rotor_state - old_rotor_state == 5)
        motor_position--;
    else if (rotor_state - old_rotor_state == -5)
        motor_position++;
    else
        motor_position += (rotor_state - old_rotor_state);
    
    old_rotor_state = rotor_state;
}

/* motor control thread sets a timer ISR, this is the handler */
void motor_ctrl_timer_isr() { motor_ctrl_thrd.signal_set(0x1); }

/* motor control thread main */
void motor_ctrl_fn()
{
    Ticker motor_control_ticker;
    Timer  timer;

    uint8_t count  = 0;

    int32_t cur_pos = 0,
        old_pos = 0,
        cur_speed,
        ys,
        yr;

    uint32_t cur_time = 0,
         old_time = 0,
         time_diff;

    float cur_err = 0.0f,
          old_err = 0.0f,
          err_diff;
    
    motor_control_ticker.attach_us(&motor_ctrl_timer_isr,100000);

    timer.start();

    while(1) {
        /* wait for the 100ms boundary */
        motor_ctrl_thrd.signal_wait(0x1);

        /* read state & timestamp */
        cur_time = timer.read();
        cur_pos  = motor_position;

        /* compute speed */
        time_diff = cur_time - old_time;
        cur_speed = (cur_pos - old_pos) / time_diff;

        /* prep values for next time through loop */
        old_time = cur_time;
        old_pos  = cur_pos;

        count = ++count % 10;

        /* update with motor status */
        /*
         * if (!count) {
         *  put_message(MSG_CUR_SPD, cur_speed);
         *  put_message(MSG_MAX_SPD, target_speed);
         *  put_message(MSG_POS, (cur_pos/6));
         *  put_message(MSG_ROT_PEN, rotations_pending);
         *  put_message(MSG_TORQUE, torque);
         * }
         */

        /* compute position error */
        cur_err  = rotations_pending - (cur_pos/6.0f);
        err_diff = cur_err - old_err;
        old_err  = cur_err;

        /* compute torques */
        ys = (int32_t) (20 * (target_speed - abs(cur_speed))) * sgn(cur_err);

        /* select minimum absolute value torque, or just take ys and spin forever */
        if (likely(!spin_forever)) {
            yr = (int32_t) ((20 * cur_err) + (40 * err_diff));
            if (cur_speed < 0)
                torque = max(ys, yr);
            else
                torque = min(ys, yr);
        } else
            torque = ys;

        /* fix torque if negative */
        if (torque < 0)
            torque = -torque, /* <- comma operator in action */
            lead   = -2;
        else
            lead = 2;

        /* cap torque */
        if (torque > MAX_TORQUE)
            torque = MAX_TORQUE;

        /* finally, give the motor a kick */
        photointerrupter_isr();
    }
}

/* parse input with sscanf() */
void parse_serial_in(char *s)
{
    /* shadow output variables so writes are guaranteed atomic */
    uint64_t new_key_;
    int32_t torque_;
    int32_t  target_speed_;
    float rotations_pending_;

    if (sscanf(s, "R%f", &rotations_pending_)) {

        rotations_pending += rotations_pending_;
        if (rotations_pending_ == 0.0f) spin_forever = true;
        else spin_forever = false;
        /* put_message(MSG_ROT_PEN, rotations_pending); */

    } else if (sscanf(s, "V%d", &target_speed_)) {

        target_speed = target_speed_;
        /* put_message(MSG_NEW_VEL, target_speed); */

    } else if (sscanf(s, "K%llx", &new_key_)) {

        new_key = new_key_;
        key_updated = true;
        /* put_message(MSG_NEW_KEY, new_key); */

    } else if (sscanf(s, "T%u", &torque_)) {
        torque = torque_;
        photointerrupter_isr(); //Give it a kick
        /* put_message(MSG_TORQUE, torque); */
    } else
        put_message(MSG_INP_ERR, 0x404);
}

void comms_in_fn()
{
    /* register serial interrupt handler */
    pc.attach(&serial_isr);

    char char_seq[CHAR_ARR_SIZE] = "";
    uint8_t buf_pos = 0;
    
    while (1) {
        if (buf_pos >= CHAR_ARR_SIZE) {
            put_message(MSG_OVERFLOW, buf_pos);
            buf_pos = 0;
        } else {
            osEvent new_event = serial_in_queue.get();
            uint8_t new_char  = (uint8_t)new_event.value.p;

            if (new_char == '\r' || new_char == '\n') {
                char_seq[buf_pos] = '\0';
                buf_pos = 0;
                parse_serial_in(char_seq);
            } else
                char_seq[buf_pos++] = new_char;
        }
    }
}

/* timer ISR to print/reset hash counts every second */
void do_hashcount()
{
    put_message(MSG_HASHCOUNT, hashcount);
    hashcount = 0;
}


/* Set a given drive state */
void motor_out(int8_t driveState, uint32_t t){

    /* Lookup the output byte from the drive state. */
    int8_t driveOut = drive_table[driveState & 0x07];

    /* Turn off first */
    if (~driveOut & 0x01) L1L.pulsewidth_us(0);
    if (~driveOut & 0x02) L1H = 1;
    if (~driveOut & 0x04) L2L.pulsewidth_us(0);
    if (~driveOut & 0x08) L2H = 1;
    if (~driveOut & 0x10) L3L.pulsewidth_us(0);
    if (~driveOut & 0x20) L3H = 1;

    /* Then turn on */
    if (driveOut & 0x01) L1L.pulsewidth_us(t);
    if (driveOut & 0x02) L1H = 0;
    if (driveOut & 0x04) L2L.pulsewidth_us(t);
    if (driveOut & 0x08) L2H = 0;
    if (driveOut & 0x10) L3L.pulsewidth_us(t);
    if (driveOut & 0x20) L3H = 0;
}

/* Convert photointerrupter inputs to a rotor state */
inline int8_t read_rotor_state()
{
    return state_map[I1 + 2*I2 + 4*I3];
}

/* Basic synchronisation routine */
int8_t motor_home()
{
    /* Put the motor in drive state 0 and wait for it to stabilise */
    motor_out(0, MAX_TORQUE);
    wait(2.0);

    /* Get the rotor state */
    return read_rotor_state();
}

/* hank rulez */