File content as of revision 6:3ae957a6437b:

#include "mbed.h"
#include "rtos.h"
#include "MODSERIAL.h"
#include "math.h"

#define BT_BAUD 9600
#define NUM_LRAS 7
#define NUM_ENS NUM_LRAS

#define PWM1 p5
#define PWM2 p6
#define PWM3 p17
#define PWM4 p20
#define PWM5 p25
#define PWM6 p26
#define PWM7 p34

#define EN1 p7
#define EN2 p8
#define EN3 p11
#define EN4 p12
#define EN5 p13
#define EN6 p29
#define EN7 p30

// bluetooth serial
// p9 - tx, p10 - rx
MODSERIAL bt(p9, p10); //only receiving pin is actually needed

//DigitalOut leds[4] = {
//    DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
//};

//int leds[NUM_LRAS];

//PwmOut lra[NUM_LRAS] = {
//    PwmOut(p5), PwmOut(p6), PwmOut(p14), PwmOut(p20),
//    PwmOut(p25), PwmOut(p26), PwmOut(p34), PwmOut(p36)
//};

PwmOut lra[NUM_LRAS] = {
    PwmOut(PWM1), PwmOut(PWM2),PwmOut(PWM3),PwmOut(PWM4),
    PwmOut(PWM5),PwmOut(PWM6),PwmOut(PWM7)
};

//DigitalOut lra_en[NUM_ENS] = {
//    DigitalOut(p7), DigitalOut(p8), DigitalOut(p11), DigitalOut(p12),
//    DigitalOut(p13), DigitalOut(p29), DigitalOut(p30), DigitalOut(p35)
//};

DigitalOut lra_en[NUM_ENS] = {
    DigitalOut(EN1), DigitalOut(EN2),DigitalOut(EN3),DigitalOut(EN4),
    DigitalOut(EN5),DigitalOut(EN6),DigitalOut(EN7)
};

int lraOn_ms[NUM_LRAS];
int lraPeriod_ms[NUM_LRAS];
float lraIntensity[NUM_LRAS];

// THREAD POINTERS
Thread* lra_thread[NUM_LRAS];
Thread* commThread;
RtosTimer* timer;
unsigned long counter_ms;

void timer_cb(void const *n)
{
    counter_ms++;
}

void lra_fun(void const *n)
{
    unsigned long startTime_ms;
    int elapsed_ms;
    int leftToWait_ms;

    while (true) {
        // Turn On LRA:
        //leds[(int)n] = 1;
        lra_en[(int)n] = 1;
        lra[(int)n] = lraIntensity[(int)n]; //set initial intensity
        startTime_ms = counter_ms; //get start time

        leftToWait_ms = lraOn_ms[(int)n];
        while( leftToWait_ms > 0) {
            //printf("time: %d\n",leftToWait_ms);
            Thread::signal_wait(0x1,(uint32_t) leftToWait_ms); //signal number, wait time
            elapsed_ms = (int)(counter_ms-startTime_ms);
            leftToWait_ms = lraOn_ms[(int)n] - elapsed_ms;
            lra[(int)n] = lraIntensity[(int)n]; //adjust intensity according to current value
        }


        //leds[(int)n] = 0;
        //Set LRA PWM to 0.5
        lra[(int)n] = 0.5f; // that turns off the motor!
        //Turn LRA Off by setting enable pin to 0
        lra_en[(int)n] = 0; // no braking happening

        //set rest time to sleep while the lra is off
        elapsed_ms = (int)(counter_ms-startTime_ms);
        leftToWait_ms = lraPeriod_ms[(int)n]-elapsed_ms;
        while( leftToWait_ms > 0) {
            //printf("time: %d\n",leftToWait_ms);
            Thread::signal_wait(0x2,(uint32_t) leftToWait_ms); //signal number, wait time
            //it woke up!
            elapsed_ms = (int)(counter_ms-startTime_ms);
            leftToWait_ms = lraPeriod_ms[(int)n] - elapsed_ms;;
        }
    }
}

// Called everytime a new character goes into
// the RX buffer. Test that character for \n
// Note, rxGetLastChar() gets the last char that
// we received but it does NOT remove it from
// the RX buffer.
void rx_cb(MODSERIAL_IRQ_INFO *q)
{
    MODSERIAL *serial = q->serial;
    if ( serial->rxGetLastChar() == '\0') {
        commThread->signal_set(0x3); //signal 3
    }
}

void commThread_cb(void const *n)
{
    int index = 0;
    int which = 0;
    char input = 0;
    float newIntensity;
    int newOnTime;
    int newTotalTime;

    // add calculations of start length and total period
//%FORMULA is
//%(input-c)/d
//
//
//% start length
//a = 1; %character start value
//b = 255; %character end value
//u = 50; %start value mapped to this lower limit
//t = 300; %end value mapped to this upper limit
//
//c=(a-u*b/t)/(1-u/t)
//d = (b-c)/t
//
//
//%%%%%%%%%%%%%%%%%%%%DIFFERENT CALCULATION
//
//% total length
//u = 300;
//t = 4000;
//c=(a-u*b/t)/(1-u/t)
//d = (b-c)/t

    while (true) {
        // Wait here until we detect the \0 going into the buffer.
        //new data in the buffer, read it out
        while(bt.readable()) {
            input = bt.getc();
            switch ( index ) {
                case 0:
                    which = input-'0';
                    index = (which < 0)? 4 : index;
                    index = (which > (NUM_LRAS-1))? 4 : index;
                    break;
                case 1:
                    // Intensity
                    input = (input < 1)? 1 : input;
                    input = (input > 255)? 255 : input;
                    // scale intensity between 0.5f to 1.0f
                    newIntensity = (float) (input+253)/508.0;
                    lraIntensity[which] = newIntensity;
                    break;
                case 2:
                    // Period Length Start
                    input = (input < 1)? 1 : input;
                    input = (input > 255)? 255 : input;
                    // scale start length between 50 to 300 - see matlab script "range_calculations.m" in git repo
                    newOnTime = (int) floor( ((input+49.8)/1.016) + 0.5); //floor(...+0.5) = round()
                    if(newOnTime!=lraOn_ms[which]) {
                        lraOn_ms[which] = newOnTime;
                        lra_thread[which]->signal_set(0x1); //signal 1
                    }
                case 3:
                    // Total Period Length
                    input = (input < 1)? 1 : input;
                    input = (input > 255)? 255 : input;
                    // scale total period length between 300 to 4000 - see matlab script "range_calculations.m" in git repo
                    newTotalTime = (int) floor( ((input+19.5946)/0.0686) +0.5); //floor(...+0.5) = round()
                    if(newTotalTime!=lraPeriod_ms[which]) {
                        lraPeriod_ms[which] = newTotalTime;
                        lra_thread[which]->signal_set(0x2); //signal 2
                    }
                    break;
                default:
                    // do nothing
                    break;
            }
            index++;
            //Thread::yield();// Pass control to next thread that is in state READY. 
        }
        index = 0; //reset index
        Thread::signal_wait(0x3); //signal 3
    }
}
int main (void)
{
    //Init communication
    bt.baud(BT_BAUD); //set baud rate of bluetooth connection
    bt.attach(&rx_cb, MODSERIAL::RxIrq); // attach callback to get '\0' command

    commThread = new Thread(commThread_cb); //Thread(commThread_cb,NULL,osPriorityBelowNormal)
    //start universal timer to count up a counter
    timer = new RtosTimer(timer_cb,osTimerPeriodic); // adjust prorioty of osTimerThread ?
    counter_ms=0;
    timer->start(1); //run timer every millisecond

    for(int i = 0; i < NUM_ENS; i++) {
        lra_en[i] = 0;
    }


    //initialize and start everything
    for(int i = 0; i < NUM_LRAS; i++) {

        //set pwm frequency
        lra[i].period_us(100);

        //initialize values
        lra[i] = 0.5f;

        //set starting vibration
        lraOn_ms[i] = 100;
        lraPeriod_ms[i] = 1000;
        lraIntensity[i] = 0.0f;
        //Set up lra threads
        lra_thread[i] = new Thread(lra_fun, (void *)i); //Thread(lra_fun, (void *)i, osPriorityNormal)
    }
}