sensory_array / Mbed 2 deprecated ymbed_haptic_belt_slave_onethread

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   USBDevice mbed mbed-rtos

main.cpp@0:7c2805142589, 2015-03-26 (annotated)

Committer:
baraki
Date:
Thu Mar 26 16:57:07 2015 +0000
Revision:
0:7c2805142589
Child:
1:2af026a7c290
single threaded slave

Who changed what in which revision?

UserRevisionLine numberNew contents of line
baraki 0:7c2805142589 1 #include "mbed.h"
baraki 0:7c2805142589 2 #include "math.h"
baraki 0:7c2805142589 3 #include "bluetoothComm.h"
baraki 0:7c2805142589 4 #define BT_BAUD 9600
baraki 0:7c2805142589 5 #define NUM_LRAS 7
baraki 0:7c2805142589 6 #define NUM_ENS NUM_LRAS
baraki 0:7c2805142589 7
baraki 0:7c2805142589 8 // bluetooth serial
baraki 0:7c2805142589 9 // p9 - tx, p10 - rx
baraki 0:7c2805142589 10 Timer timer;
baraki 0:7c2805142589 11
baraki 0:7c2805142589 12 //DigitalOut leds[4] = {
baraki 0:7c2805142589 13 // DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
baraki 0:7c2805142589 14 //};
baraki 0:7c2805142589 15
baraki 0:7c2805142589 16 //int leds[NUM_LRAS];
baraki 0:7c2805142589 17
baraki 0:7c2805142589 18 PwmOut lra[NUM_LRAS] = {
baraki 0:7c2805142589 19 PwmOut(p5), PwmOut(p6),PwmOut(p17),PwmOut(p20),
baraki 0:7c2805142589 20 PwmOut(p25),PwmOut(p26),PwmOut(p34)
baraki 0:7c2805142589 21 };
baraki 0:7c2805142589 22
baraki 0:7c2805142589 23
baraki 0:7c2805142589 24 DigitalOut lra_en[NUM_ENS] = {
baraki 0:7c2805142589 25 DigitalOut(p7), DigitalOut(p8),DigitalOut(p11),DigitalOut(p12),
baraki 0:7c2805142589 26 DigitalOut(p13),DigitalOut(p29),DigitalOut(p30)
baraki 0:7c2805142589 27 };
baraki 0:7c2805142589 28
baraki 0:7c2805142589 29 int lraOn_ms[NUM_LRAS];
baraki 0:7c2805142589 30 int lraPeriod_ms[NUM_LRAS];
baraki 0:7c2805142589 31 float lraIntensity[NUM_LRAS];
baraki 0:7c2805142589 32
baraki 0:7c2805142589 33 void processData(void const *n)
baraki 0:7c2805142589 34 {
baraki 0:7c2805142589 35 int index = 0;
baraki 0:7c2805142589 36 int which = 0;
baraki 0:7c2805142589 37 char input = 0;
baraki 0:7c2805142589 38 float newIntensity;
baraki 0:7c2805142589 39 int newOnTime;
baraki 0:7c2805142589 40 int newTotalTime;
baraki 0:7c2805142589 41
baraki 0:7c2805142589 42 while(bt.readable()) {
baraki 0:7c2805142589 43 input = bt.getc();
baraki 0:7c2805142589 44
baraki 0:7c2805142589 45 switch ( index ) {
baraki 0:7c2805142589 46 case 0: {
baraki 0:7c2805142589 47 which = input-('0');
baraki 0:7c2805142589 48 //index = (which < 0)? 4 : index;
baraki 0:7c2805142589 49 //index = (which > (NUM_LRAS-1))? 4 : index;
baraki 0:7c2805142589 50 which = (which < 0)? int(0) : which;
baraki 0:7c2805142589 51 which = (which > (NUM_LRAS-1))? (NUM_LRAS-1) : which;
baraki 0:7c2805142589 52 //bt.putc('a');
baraki 0:7c2805142589 53 //bt.putc(input);
baraki 0:7c2805142589 54 //bt.putc(0);
baraki 0:7c2805142589 55 break;
baraki 0:7c2805142589 56 }
baraki 0:7c2805142589 57 case 1: {
baraki 0:7c2805142589 58 // Intensity
baraki 0:7c2805142589 59 //input = (input < 1)? char(1) : input;
baraki 0:7c2805142589 60 //input = (input > 255)? char(255) : input;
baraki 0:7c2805142589 61 // scale intensity between 0.5f to 1.0f
baraki 0:7c2805142589 62 newIntensity = (float) (input+253)/508.0;
baraki 0:7c2805142589 63 lraIntensity[which] = newIntensity;
baraki 0:7c2805142589 64 //bt.putc('b');
baraki 0:7c2805142589 65 //bt.printf("%f",newIntensity);
baraki 0:7c2805142589 66 //bt.putc(0);
baraki 0:7c2805142589 67 break;
baraki 0:7c2805142589 68 }
baraki 0:7c2805142589 69 case 2: {
baraki 0:7c2805142589 70 // Period Length Start
baraki 0:7c2805142589 71 input = (input < 1)? 1 : input;
baraki 0:7c2805142589 72 input = (input > 255)? 255 : input;
baraki 0:7c2805142589 73 // scale start length between 50 to 300 - see matlab script "range_calculations.m" in git repo
baraki 0:7c2805142589 74 newOnTime = (int) floor( ((input+49.8)/1.016) + 0.5); //floor(...+0.5) = round()
baraki 0:7c2805142589 75 if(newOnTime!=lraOn_ms[which]) {
baraki 0:7c2805142589 76 lraOn_ms[which] = newOnTime;
baraki 0:7c2805142589 77 }
baraki 0:7c2805142589 78 //bt.putc('c');
baraki 0:7c2805142589 79 //bt.printf("%d",input);
baraki 0:7c2805142589 80 //bt.putc(0);
baraki 0:7c2805142589 81 }
baraki 0:7c2805142589 82 case 3: {
baraki 0:7c2805142589 83 // Total Period Length
baraki 0:7c2805142589 84 input = (input < 1)? 1 : input;
baraki 0:7c2805142589 85 input = (input > 255)? 255 : input;
baraki 0:7c2805142589 86 // scale total period length between 300 to 4000 - see matlab script "range_calculations.m" in git repo
baraki 0:7c2805142589 87 newTotalTime = (int) floor( ((input+19.5946)/0.0686) +0.5); //floor(...+0.5) = round()
baraki 0:7c2805142589 88 if(newTotalTime!=lraPeriod_ms[which]) {
baraki 0:7c2805142589 89 lraPeriod_ms[which] = newTotalTime;
baraki 0:7c2805142589 90 }
baraki 0:7c2805142589 91 //bt.putc('d');
baraki 0:7c2805142589 92 //bt.printf("%d",input);
baraki 0:7c2805142589 93 //bt.putc(0);
baraki 0:7c2805142589 94 break;
baraki 0:7c2805142589 95 }
baraki 0:7c2805142589 96 default: {
baraki 0:7c2805142589 97 // do nothing
baraki 0:7c2805142589 98 break;
baraki 0:7c2805142589 99 }
baraki 0:7c2805142589 100 }
baraki 0:7c2805142589 101 index++;
baraki 0:7c2805142589 102 //Thread::yield();// Pass control to next thread that is in state READY.
baraki 0:7c2805142589 103 }
baraki 0:7c2805142589 104 //bt.putc(0);
baraki 0:7c2805142589 105 index = 0; //reset index
baraki 0:7c2805142589 106 }
baraki 0:7c2805142589 107 int main (void)
baraki 0:7c2805142589 108 {
baraki 0:7c2805142589 109 //Init communication
baraki 0:7c2805142589 110 robotSetup(BT_BAUD); //set baud rate of bluetooth connection
baraki 0:7c2805142589 111
baraki 0:7c2805142589 112 //start universal timer to count up a counter
baraki 0:7c2805142589 113 timer.start();
baraki 0:7c2805142589 114 int counter_ms = 0;
baraki 0:7c2805142589 115
baraki 0:7c2805142589 116 //initialize and start everything
baraki 0:7c2805142589 117 char btData[50];
baraki 0:7c2805142589 118 unsigned long startTime_ms[NUM_LRAS];
baraki 0:7c2805142589 119 int elapsed_ms[NUM_LRAS];
baraki 0:7c2805142589 120 int leftToWait_ms[NUM_LRAS];
baraki 0:7c2805142589 121 bool isOn[NUM_LRAS];
baraki 0:7c2805142589 122 for(int i = 0; i < NUM_LRAS; i++) {
baraki 0:7c2805142589 123 //set pwm frequency
baraki 0:7c2805142589 124 lra[i].period_us(100);
baraki 0:7c2805142589 125 //initialize values
baraki 0:7c2805142589 126 lra[i] = 0.5f;
baraki 0:7c2805142589 127 //set starting vibration
baraki 0:7c2805142589 128 lraOn_ms[i] = 100;
baraki 0:7c2805142589 129 lraPeriod_ms[i] = 1000;
baraki 0:7c2805142589 130 lraIntensity[i] = 0.0f;
baraki 0:7c2805142589 131
baraki 0:7c2805142589 132 lra_en[i] = 0;
baraki 0:7c2805142589 133 lra[i] = lraIntensity[i]; //set initial intensity
baraki 0:7c2805142589 134 startTime_ms[i] = counter_ms; //get start time
baraki 0:7c2805142589 135 isOn[i] = false;
baraki 0:7c2805142589 136 leftToWait_ms[i] = lraOn_ms[i];
baraki 0:7c2805142589 137 }
baraki 0:7c2805142589 138 while(1){
baraki 0:7c2805142589 139 if(getBluetoothData()){ //if the bluetooth data has finished sending (there is a \0 detected)
baraki 0:7c2805142589 140 //read buffer
baraki 0:7c2805142589 141 //parse data
baraki 0:7c2805142589 142 strcpy(returnBluetoothData(), btData, 50);
baraki 0:7c2805142589 143 int len = length(btData);
baraki 0:7c2805142589 144 }
baraki 0:7c2805142589 145 counter_ms = timer.read_ms();
baraki 0:7c2805142589 146 for(int n=0;n<NUM_LRAS;n++){
baraki 0:7c2805142589 147 // lra_fun
baraki 0:7c2805142589 148 // Turn On LRA:
baraki 0:7c2805142589 149 //leds[(int)n] = 1;
baraki 0:7c2805142589 150
baraki 0:7c2805142589 151 if(isOn[n]) {
baraki 0:7c2805142589 152 elapsed_ms[n] = (int)(counter_ms-startTime_ms[n]);
baraki 0:7c2805142589 153 leftToWait_ms[n] = lraOn_ms[n] - elapsed_ms[n];
baraki 0:7c2805142589 154 if(leftToWait_ms[n] > 0) {
baraki 0:7c2805142589 155 lra[n] = lraIntensity[n]; //adjust intensity according to current value
baraki 0:7c2805142589 156
baraki 0:7c2805142589 157 } else {
baraki 0:7c2805142589 158 isOn[n] = false;
baraki 0:7c2805142589 159 //Set LRA PWM to 0.5
baraki 0:7c2805142589 160 lra[n] = 0.5f; // that turns off the motor!
baraki 0:7c2805142589 161 //Turn LRA Off by setting enable pin to 0
baraki 0:7c2805142589 162 lra_en[n] = 0; // no braking happening
baraki 0:7c2805142589 163 }
baraki 0:7c2805142589 164 }
baraki 0:7c2805142589 165 else {
baraki 0:7c2805142589 166 //printf("time: %d\n",leftToWait_ms);
baraki 0:7c2805142589 167 elapsed_ms[n] = (int)(counter_ms-startTime_ms[n]);
baraki 0:7c2805142589 168 leftToWait_ms[n] = lraPeriod_ms[n] - elapsed_ms[n];
baraki 0:7c2805142589 169 if(leftToWait_ms[n] < 0) {
baraki 0:7c2805142589 170 isOn[n] = true;
baraki 0:7c2805142589 171 //Set LRA PWM to desired intensity
baraki 0:7c2805142589 172 lra[n] = lraIntensity[n]; // that turns on the motor!
baraki 0:7c2805142589 173 //Turn LRA On by setting enable pin to 1
baraki 0:7c2805142589 174 lra_en[n] = 1;
baraki 0:7c2805142589 175 }
baraki 0:7c2805142589 176 }
baraki 0:7c2805142589 177 }
baraki 0:7c2805142589 178 }
baraki 0:7c2805142589 179 }