File content as of revision 4:91d5db5e8c9f:

#include "mbed.h"
#include "math.h"
#include "bluetoothComm.h"
#define BT_BAUD 9600
#define NUM_LRAS 6
#define NUM_ENS NUM_LRAS

// bluetooth serial
// p9 - tx, p10 - rx
Timer timer;

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

//int leds[NUM_LRAS];

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

//int lra[NUM_LRAS];
//int lra_en[NUM_ENS];

DigitalOut lra_en[NUM_ENS] = {
   DigitalOut(p7), DigitalOut(p8),DigitalOut(p12),
    DigitalOut(p13),DigitalOut(p29),DigitalOut(p30)
};
DigitalOut en3(p11);
int lraOn_ms[NUM_LRAS];
int lraPeriod_ms[NUM_LRAS];
float lraIntensity[NUM_LRAS];

void processData(char *n)
{
    int i = 0;
    int index = 0;
    int which = 0;
    float newIntensity;
    int newOnTime;
    int newTotalTime;
    
    char input = n[i];

    while(input != '\0') {
        switch ( index ) {
            case 0: {
                which = input-('0');
                //index = (which < 0)? 4 : index;
                //index = (which > (NUM_LRAS-1))? 4 : index;
                which = (which < 0)? int(0) : which;
                which = (which > (NUM_LRAS-1))? (NUM_LRAS-1) : which;
                break;
            }
            case 1: {
                // Intensity
                //input = (input < 1)? char(1) : input;
                //input = (input > 255)? char(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;
                }
            }
            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;
                }
                break;
            }
            default: {
                // do nothing
                break;
            }
        }
        index++;
        i++;
        input = n[i];
     }
}

int main (void)
{
    en3 = 0;
    //Init communication
    robotSetup(BT_BAUD); //set baud rate of bluetooth connection
    //start universal timer to count up a counter
    timer.start();
    timer.reset();
    int counter_ms = timer.read_ms();

    //initialize and start everything
    int startTime_ms[NUM_LRAS];
    int elapsed_ms[NUM_LRAS];
    int leftToWait_ms[NUM_LRAS];
    bool isOn[NUM_LRAS];
    for(int i = 0; i < NUM_LRAS; i++) {
        //set pwm frequency
        lra[i].period_us(90);
        //initialize values
        //set starting vibration
        lraOn_ms[i] = 100;
        lraPeriod_ms[i] = 1000;
        lraIntensity[i] = 0.5f;
        
        lra_en[i] = 0;
        lra[i] = lraIntensity[i]; //set initial intensity
        startTime_ms[i] = counter_ms; //get start time
        isOn[i] = false;
        leftToWait_ms[i] = lraOn_ms[i];
    }
    while(1){
        if(getBluetoothData()){ //if the bluetooth data has finished sending (there is a \0 detected)
            //console1.printf("btData: ");
            //console1.printf(bluetoothData);
            //console1.printf("\n");
            processData(bluetoothData);
            int num = bluetoothData[0] - '0';
            if(num == 1){
                //console1.printf("Motor#: %d | ", num);
                //console1.printf("Intensity : %f | ", lraIntensity[num]);
                //console1.printf("lraOn : %d | ",lraOn_ms[num]);
                //console1.printf("lraPeriod : %d \n", lraPeriod_ms[num]);
            }
        }
        for(int n=0;n<NUM_LRAS;n++){
            // lra_fun
            counter_ms = timer.read_ms();
            elapsed_ms[n] = (int)(counter_ms-startTime_ms[n]);
            if(isOn[n]) {
                leftToWait_ms[n] = lraOn_ms[n] - elapsed_ms[n];
                if(leftToWait_ms[n] > 0) {
                    lra[n] = lraIntensity[n]; //adjust intensity according to current value

                } else {
                    console1.printf("Turning motor off\n");
                    isOn[n] = false;
                    //Set LRA PWM to 0.5
                    lra[n] = 0.5f; // that turns off the motor!
                    //Turn LRA Off by setting enable pin to 0
                    lra_en[n] = 0; // no braking happening
                }
            }
            else {
                leftToWait_ms[n] = lraPeriod_ms[n] - elapsed_ms[n];    
                if(n == 1){
                    console1.printf("leftToWait_ms[n] = %d\n",leftToWait_ms[n]);
                    console1.printf("elapsed_ms[n] = %d\n",elapsed_ms[n]);
                   }
                if(leftToWait_ms[n] < 0) {
                    console1.printf("Turning motor on\n");
                    isOn[n] = true;
                    //Set LRA PWM to desired intensity
                    lra[n] = lraIntensity[n]; // that turns on the motor!
                    //Turn LRA On by setting enable pin to 1
                    lra_en[n] = 1; 
                    startTime_ms[n] = timer.read_ms();
                }
            }
        }
    }
}