File content as of revision 0:5b9f87a086ce:

#include "mbed.h"
#include "math.h"
#include "bluetoothComm.h"
#include "NeoStrip.h"

#define BT_BAUD 9600
#define NUM_LRAS 5
#define NUM_ENS NUM_LRAS
#define NUM_LEVELS 7
#define NUM_PULSES 5

#define NUM_LIDAR 7 //number of lidar

#define LED_NUM NUM_LIDAR
#define LED_PIN p16

NeoStrip leds(LED_PIN, LED_NUM);

PwmOut lra[NUM_LRAS] = {
    PwmOut(p25), PwmOut(p24),PwmOut(p23),PwmOut(p22),
    PwmOut(p21)
};

DigitalOut myLED(LED1);

Ticker motorPulse;
Timeout motorOff;

// given a number between 0 and NUM_PULSES, returns the number of times
// the LRA should be pulsed during the maximum pulse interval
int pulse[NUM_PULSES] = {9, 8, 4, 2, 1};

// given a number between 0 and NUM_LEVELS, returns the
// corresponding intensity of the LRA
float intensity[8] = {0.0, 150.0/255.0, 170.0/255.0, 190.0/255.0,210.0/255.0,230.0/255.0, 250.0/255.0,255.0/255.0};

DigitalOut lra_en[NUM_ENS] = {
   DigitalOut(p7), DigitalOut(p8),DigitalOut(p12),DigitalOut(p13),
    DigitalOut(p14)
};

//length of the longest period in milliseconds
int longPeriod=600; //1024
int shortPeriod=longPeriod/pulse[0];
int onPeriod=60; //100

bool isOn[NUM_LRAS+2];
volatile int lraPulse[NUM_LRAS + 2];
volatile int lraCount;
volatile char lraIntensity[NUM_LRAS + 2];
// currently three modes of vibrating
// vibration period is divided into three sections
// motor can be: on/mid ; on/off ; on/mid
// MODE 1: ON ON ON
// MODE 2: MID OFF ON
// MODE 3: ON OFF MID
int lraMode[NUM_LRAS+2];

Serial pc(USBTX,USBRX);

void motorOff3_cb(void);
void motorOff2_cb(void);
void motorOff1_cb(void);
void processData(char *n);

void setColor(int num, int power){
    int lednum = num*2;
    if(num == 5)
        lednum = 3;
    if(num == 6)
        lednum = 5;
    if(power >= 240)
         leds.setPixel(lednum, 100, 0, 0);
    else if(power >= 220 && power < 240)
        leds.setPixel(lednum, 0, 100,0);
    else if(power >= 200 && power < 220)
        leds.setPixel(lednum,100,0,100);
    else if(power >= 100 && power < 200)
        leds.setPixel(lednum,0,0,100);
    else
        leds.setPixel(lednum,0,0,0);
}

void processData(char *n) {
    //try to take these variables out and make them globals to speed processing
    int i = 0;
    int index = 0;
    
    char input =  n[i];
    
    unsigned char newIntensity = 0;
    
    int m1pulse = 0;
    int m1int = 0;
    
    int m2pulse = 0;
    int m2int = 0;
    
    while(input !='\0'){
        switch ( index ) {
            case 0: {
                // bits 0-2: m1 pulse
                // bits 3-5: m1 intensity
                // bits 6-7: first 2 bits of m1 mode
                lraPulse[0] = pulse[(int)((input & 0xE0) >> 5) - 1]; //0b11100000
                m1pulse = ((input & 0xE0) >> 5);
                m1int = ((input & 0x1C) >> 2);
                lraIntensity[0] = intensity[(int) ((input & 0x1C) >> 2)]; //0b00011100
                setColor(0, lraIntensity[0]);
                lraMode[0] = (input & 0x3) << 1; //0b00000011
                break;
            }
            case 1: {
                // bit 0: last bit of m1 mode
                // bits 1-3: m2 pulse
                // bits 4-6: m2 intensity
                // bit 7: first bit of m2 mode
                lraMode[0] = lraMode[0] | ((input & 0x80) >> 7); //0b10000000
                //printf("m1 pulse: %d, intensity: %d, mode: %d \n",m1pulse,m1int,lraMode[0]);
                printf("m1 pulse: %d, intensity: %f, mode: %d \n",lraPulse[0],lraIntensity[0],lraMode[0]);
                m2pulse = ((input & 0x70) >> 4);
                m2int = ((input & 0xE) >> 1);
                lraPulse[1] = pulse[(int)((input & 0x70) >> 4) - 1]; //0b01110000
                lraIntensity[1] = intensity[(int)((input & 0xE) >> 1)]; //0b00001110
                setColor(1, lraIntensity[1]);
                lraMode[1] = (input & 0x01) << 2; //0b00000001
                break;
            }
            case 2: {
                // bits 0-1: last 2 bits of m2 mode
                // bits 2-4: m3 pulse
                // bits 5-7: m3 intensity
                lraMode[1] = lraMode[1] | ((input & 0xC0) >> 6); //0b11000000
                //printf("m2 pulse: %d, intensity: %d, mode: %d \n",m2pulse,m2int,lraMode[1]);
                printf("m2 pulse: %d, intensity: %f, mode: %d \n",lraPulse[1],lraIntensity[1],lraMode[1]);
                lraPulse[2] = pulse[(int)((input & 0x38) >> 3) - 1]; //0b00111000
                lraIntensity[2] = intensity[(int)(input & 0x7)]; //0b00000111
                setColor(2, lraIntensity[2]);
                break;
            }
            case 3: {
                // bits 0-2: m3 mode
                // bits 3-5: m4 pulse
                // bits 6-7: first 2 bits of m4 intensity
                lraMode[2] = (input & 0xE0) >> 5; //0b11100000
                //printf("m3 pulse: %d, intensity: %d, mode: %d \n",((input & 0x38) >> 3),(input & 0x7),lraMode[2]);
                printf("m3 pulse: %d, intensity: %f, mode: %d \n",lraPulse[2],lraIntensity[2],lraMode[2]);
                lraPulse[3] = pulse[(int)((input & 0x1C) >> 2) - 1]; //0b00011100
                newIntensity = (input & 0x03) << 1; //0b00000011
                break;
            }
            case 4: {
                // bit 0: last bit of m4 intensity
                // bits 1-3: m4 mode
                // bits 4-6: m5 pulse
                // bit 7: first bit of m5 intensity
                newIntensity = newIntensity | ((input & 0x80) >> 7); //0b10000000
                lraIntensity[3] = intensity[(int)newIntensity];
                setColor(3, lraIntensity[3]);
                lraMode[3] = (input & 0x70) >> 4; //0b01110000
                lraPulse[4] = pulse[(int)((input & 0xE) >> 1) - 1]; //0b00001110
                newIntensity = (input & 0x01) << 2; //0b00000001
                break;
            }
            case 5: {
                // bits 0-1: last 2 bits of m5 intensity
                // bits 2-4: m5 mode
                // bits 5-7: 3 bit checksum supposedly
                newIntensity = newIntensity | ((input & 0xC0) >> 6); //0b11000000
                lraIntensity[4] = intensity[(int)newIntensity];
                setColor(4, lraIntensity[4]);
                lraMode[4] = (input & 0x38) >> 3; //0b00111000
                break;
            }
            case 6: { //UP
                // bits 0-2: m3 pulse UP
                // bits 3-5: m3 intensity UP
                lraPulse[5] = pulse[(int)((input & 0xE0) >> 5) - 1]; //0b11100000
                //m3pulse = ((input & 0xE0) >> 5);
                //m3int = ((input & 0x1C) >> 2);
                lraIntensity[5] = intensity[(int) ((input & 0x1C) >> 2)]; //0b00011100
                setColor(5, lraIntensity[5]);
                break;
            }
            case 7: { //DOWN
                // bits 0-2: m3 pulse DOWN
                // bits 3-5: m3 intensity1 mode DOWN
                lraPulse[6] = pulse[(int)((input & 0xE0) >> 5) - 1]; //0b11100000
                //m3pulse = ((input & 0xE0) >> 5);
                //m3int = ((input & 0x1C) >> 2);
                lraIntensity[6] = intensity[(int) ((input & 0x1C) >> 2)]; //0b00011100
                setColor(6, lraIntensity[6]);
                
                //if lraPulse 5 or 6 are >3, that means set middle motor to highest pulse
                if(lraPulse[5] < 4 || lraPulse[6] < 4){
                    lraPulse[2] = pulse[NUM_PULSES-1];
                    lraIntensity[2] = intensity[NUM_LEVELS];
                }
                break;
            }
            default: {
                // do nothing
                break;
            }
        }
        index++;
        i++;
        input = n[i];
     }
}

void motorPulse_cb(void){      
    pc.printf("hi\n");
    for(int n=0; n<NUM_LRAS; n++) {
        //pulse motor if lraCount is 0
        if(lraCount % lraPulse[n] == 0) {
            isOn[n] = true;
            //Set LRA PWM to desired intensity
            //if((lraMode[n] == 1) || (lraMode[n] == 3)) 
            lra[n] = lraIntensity[n]; //full intensity 
            pc.printf("M%d : %f\n",n,lraIntensity[n]);
            //else if(lraMode[n] == 2)
             //   lra[n] = lraIntensity[n] - 0.1; //"mid" intensity
            //Turn LRA On by setting enable pin to 1
            lra_en[n] = 1;
        }
    }
    lraCount++;
    if(lraCount > 8)
        lraCount = 1;
    motorOff.detach();
    motorOff.attach(&motorOff3_cb,((float)onPeriod)/1300.0);
}

int main (void)
{
    //Init communication
    robotSetup(BT_BAUD); //set baud rate of bluetooth connection
    pc.baud(9600);
    
    leds.clear();
    leds.setBrightness(0.15);

    //initialize and start everything
    lraCount = 0;
    for(int i = 0; i < NUM_LRAS; i++) {
        //set pwm frequency
        lra[i].period_us(90);
        //initialize values
        //set starting vibration
        lraIntensity[i] = 0.5f;
        lraPulse[i] = 0;
        lraMode[i] = 0;
        
        lra_en[i] = 0;
        lra[i] = lraIntensity[i]; //set initial intensity
        isOn[i] = false;
    }
    
    motorPulse.attach(&motorPulse_cb, ((float) shortPeriod)/1000.0);
    
    while(1){
        if(getBluetoothData()){ //if the bluetooth data has finished sending (there is a \0 detected)
            processData(bluetoothData);
        }
    }
}
void motorOff1_cb(void) {
    for(int n=0;n<NUM_LRAS;n++){
        if(isOn[n]) {
            if(lraMode[n] == 2 || lraMode[n] == 3){//turn off motor
                //do NOT set isOn to 0
                //isOn is used to tell if the motor is in the midst of a pulse
                //even if it is turned off during this phase
                //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
            }
            //if mode is 1, leave it on
        }
        //motorOff[n].detach();
        //motorOff[n].attach(&motorOff2_cb,((float)onPeriod)/3000.0);
    }
}

void motorOff2_cb(void) {
    for(int n=0;n<NUM_LRAS;n++){
        if(isOn[n]) {
            if(lraMode[n] == 2){ //turn motor back on
                lra[n] = lraIntensity[n];
                lra_en[n] = 1;
            }
            else if(lraMode[n] == 3){//turn motor to mid intensity
                lra[n] = lraIntensity[n] - 0.1;
                lra_en[n] = 1;
            }
        }
        //motorOff[n].detach();
        //motorOff[n].attach(&motorOff3_cb,((float)onPeriod)/3000.0);
    }
}

void motorOff3_cb(void) {
    lra[0] = 0.2f;
    lra[1] = 0.2f;
    lra[2] = 0.2f;
    lra[3] = 0.2f;
    lra[4] = 0.2f;
    lra_en[0] = 0.2f;
    lra_en[1] = 0.2f;
    lra_en[2] = 0.2f;
    lra_en[3] = 0.2f;
    lra_en[4] = 0.2f;
    //for(int n=0;n<NUM_LRAS;n++){
    //    if(isOn[n]) {
    //        //turn em off
    //        isOn[n] = 0;
    //        lra[n] = 0.2f;
    //        lra_en[n] = 0;
    //    }
    //}
}