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@4:91d5db5e8c9f, 2015-03-27 (annotated)

Committer:
baraki
Date:
Fri Mar 27 14:36:29 2015 +0000
Revision:
4:91d5db5e8c9f
Parent:
3:6a249e8de7d8
Child:
5:301515ccaddc
working code. Uses Timer, so pwm pin 17, 18, and 30 won't work. So only 6 LRAs work on this code.

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