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@1:2af026a7c290, 2015-03-26 (annotated)

Committer:
baraki
Date:
Thu Mar 26 19:51:11 2015 +0000
Revision:
1:2af026a7c290
Parent:
0:7c2805142589
Child:
2:1a0d675eaa6f
seems to work, using many printfs to debug

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 1:2af026a7c290 33 void processData(char *n)
baraki 0:7c2805142589 34 {
baraki 1:2af026a7c290 35 int i = 0;
baraki 0:7c2805142589 36 int index = 0;
baraki 0:7c2805142589 37 int which = 0;
baraki 0:7c2805142589 38 float newIntensity;
baraki 0:7c2805142589 39 int newOnTime;
baraki 0:7c2805142589 40 int newTotalTime;
baraki 1:2af026a7c290 41
baraki 1:2af026a7c290 42 char input = n[i];
baraki 0:7c2805142589 43
baraki 1:2af026a7c290 44 while(input != '\0') {
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 1:2af026a7c290 102 i++;
baraki 1:2af026a7c290 103 input = n[i];
baraki 1:2af026a7c290 104 }
baraki 0:7c2805142589 105 }
baraki 1:2af026a7c290 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 //start universal timer to count up a counter
baraki 0:7c2805142589 112 timer.start();
baraki 0:7c2805142589 113 int counter_ms = 0;
baraki 0:7c2805142589 114
baraki 0:7c2805142589 115 //initialize and start everything
baraki 0:7c2805142589 116 char btData[50];
baraki 0:7c2805142589 117 unsigned long startTime_ms[NUM_LRAS];
baraki 0:7c2805142589 118 int elapsed_ms[NUM_LRAS];
baraki 0:7c2805142589 119 int leftToWait_ms[NUM_LRAS];
baraki 0:7c2805142589 120 bool isOn[NUM_LRAS];
baraki 0:7c2805142589 121 for(int i = 0; i < NUM_LRAS; i++) {
baraki 0:7c2805142589 122 //set pwm frequency
baraki 0:7c2805142589 123 lra[i].period_us(100);
baraki 0:7c2805142589 124 //initialize values
baraki 0:7c2805142589 125 lra[i] = 0.5f;
baraki 0:7c2805142589 126 //set starting vibration
baraki 0:7c2805142589 127 lraOn_ms[i] = 100;
baraki 0:7c2805142589 128 lraPeriod_ms[i] = 1000;
baraki 0:7c2805142589 129 lraIntensity[i] = 0.0f;
baraki 0:7c2805142589 130
baraki 0:7c2805142589 131 lra_en[i] = 0;
baraki 0:7c2805142589 132 lra[i] = lraIntensity[i]; //set initial intensity
baraki 0:7c2805142589 133 startTime_ms[i] = counter_ms; //get start time
baraki 0:7c2805142589 134 isOn[i] = false;
baraki 0:7c2805142589 135 leftToWait_ms[i] = lraOn_ms[i];
baraki 0:7c2805142589 136 }
baraki 0:7c2805142589 137 while(1){
baraki 0:7c2805142589 138 if(getBluetoothData()){ //if the bluetooth data has finished sending (there is a \0 detected)
baraki 0:7c2805142589 139 //read buffer
baraki 0:7c2805142589 140 //parse data
baraki 0:7c2805142589 141 strcpy(returnBluetoothData(), btData, 50);
baraki 1:2af026a7c290 142 console1.printf("btData: ");
baraki 1:2af026a7c290 143 console1.printf(bluetoothData);
baraki 1:2af026a7c290 144 console1.printf("\n");
baraki 1:2af026a7c290 145 processData(btData);
baraki 1:2af026a7c290 146 if(counter_ms % 100 == 0){
baraki 1:2af026a7c290 147 int num = btData[0] - '0';
baraki 1:2af026a7c290 148 console1.printf("processed Data: ");
baraki 1:2af026a7c290 149 console1.printf("Motor#: %d | ", num);
baraki 1:2af026a7c290 150 console1.printf("Intensity : ");
baraki 1:2af026a7c290 151 console1.printf("%f | ", lraIntensity[num]);
baraki 1:2af026a7c290 152 console1.printf("lraOn : ");
baraki 1:2af026a7c290 153 console1.printf("%d | ",lraOn_ms[num]);
baraki 1:2af026a7c290 154 console1.printf("lraPeriod : ");
baraki 1:2af026a7c290 155 console1.printf("%d ", lraPeriod_ms[num]);
baraki 1:2af026a7c290 156 console1.printf("\n");
baraki 1:2af026a7c290 157 }
baraki 0:7c2805142589 158 }
baraki 0:7c2805142589 159 counter_ms = timer.read_ms();
baraki 0:7c2805142589 160 for(int n=0;n<NUM_LRAS;n++){
baraki 0:7c2805142589 161 // lra_fun
baraki 0:7c2805142589 162 // Turn On LRA:
baraki 0:7c2805142589 163 //leds[(int)n] = 1;
baraki 0:7c2805142589 164
baraki 0:7c2805142589 165 if(isOn[n]) {
baraki 0:7c2805142589 166 elapsed_ms[n] = (int)(counter_ms-startTime_ms[n]);
baraki 0:7c2805142589 167 leftToWait_ms[n] = lraOn_ms[n] - elapsed_ms[n];
baraki 0:7c2805142589 168 if(leftToWait_ms[n] > 0) {
baraki 0:7c2805142589 169 lra[n] = lraIntensity[n]; //adjust intensity according to current value
baraki 0:7c2805142589 170
baraki 0:7c2805142589 171 } else {
baraki 0:7c2805142589 172 isOn[n] = false;
baraki 0:7c2805142589 173 //Set LRA PWM to 0.5
baraki 0:7c2805142589 174 lra[n] = 0.5f; // that turns off the motor!
baraki 0:7c2805142589 175 //Turn LRA Off by setting enable pin to 0
baraki 0:7c2805142589 176 lra_en[n] = 0; // no braking happening
baraki 0:7c2805142589 177 }
baraki 0:7c2805142589 178 }
baraki 0:7c2805142589 179 else {
baraki 0:7c2805142589 180 //printf("time: %d\n",leftToWait_ms);
baraki 0:7c2805142589 181 elapsed_ms[n] = (int)(counter_ms-startTime_ms[n]);
baraki 0:7c2805142589 182 leftToWait_ms[n] = lraPeriod_ms[n] - elapsed_ms[n];
baraki 0:7c2805142589 183 if(leftToWait_ms[n] < 0) {
baraki 0:7c2805142589 184 isOn[n] = true;
baraki 0:7c2805142589 185 //Set LRA PWM to desired intensity
baraki 0:7c2805142589 186 lra[n] = lraIntensity[n]; // that turns on the motor!
baraki 0:7c2805142589 187 //Turn LRA On by setting enable pin to 1
baraki 0:7c2805142589 188 lra_en[n] = 1;
baraki 0:7c2805142589 189 }
baraki 0:7c2805142589 190 }
baraki 0:7c2805142589 191 }
baraki 0:7c2805142589 192 }
baraki 0:7c2805142589 193 }