sensory_array / Mbed 2 deprecated ymbed_haptic_belt_slave

Dependencies:   MODSERIAL mbed-rtos mbed

main.cpp@5:10b1ff176798, 2015-03-23 (annotated)

Committer:
rkk
Date:
Mon Mar 23 20:31:04 2015 +0000
Revision:
5:10b1ff176798
Parent:
4:7a203e4c0cf9
Child:
6:3ae957a6437b
added comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rkk 0:5e236dc97961 1 #include "mbed.h"
rkk 0:5e236dc97961 2 #include "rtos.h"
rkk 0:5e236dc97961 3 #include "MODSERIAL.h"
rkk 0:5e236dc97961 4 #include "math.h"
rkk 0:5e236dc97961 5
rkk 0:5e236dc97961 6 #define BT_BAUD 9600
rkk 2:5914e8cbf992 7 #define NUM_LRAS 7
rkk 3:d97aeeb78f1c 8 #define NUM_ENS NUM_LRAS
rkk 2:5914e8cbf992 9
rkk 0:5e236dc97961 10 // bluetooth serial
rkk 0:5e236dc97961 11 // p9 - tx, p10 - rx
rkk 0:5e236dc97961 12 MODSERIAL bt(p9, p10); //only receiving pin is actually needed
rkk 0:5e236dc97961 13
rkk 2:5914e8cbf992 14 //DigitalOut leds[4] = {
rkk 1:f213d3a65eb4 15 // DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
rkk 1:f213d3a65eb4 16 //};
rkk 1:f213d3a65eb4 17
rkk 5:10b1ff176798 18 //int leds[NUM_LRAS];
rkk 2:5914e8cbf992 19
rkk 2:5914e8cbf992 20 //PwmOut lra[NUM_LRAS] = {
rkk 2:5914e8cbf992 21 // PwmOut(p5), PwmOut(p6), PwmOut(p14), PwmOut(p20),
rkk 2:5914e8cbf992 22 // PwmOut(p25), PwmOut(p26), PwmOut(p34), PwmOut(p36)
rkk 2:5914e8cbf992 23 //};
rkk 2:5914e8cbf992 24
rkk 1:f213d3a65eb4 25 PwmOut lra[NUM_LRAS] = {
rkk 2:5914e8cbf992 26 PwmOut(p5), PwmOut(p6),PwmOut(p17),PwmOut(p20),
rkk 2:5914e8cbf992 27 PwmOut(p25),PwmOut(p26),PwmOut(p34)
rkk 0:5e236dc97961 28 };
rkk 0:5e236dc97961 29
rkk 2:5914e8cbf992 30 //DigitalOut lra_en[NUM_ENS] = {
rkk 2:5914e8cbf992 31 // DigitalOut(p7), DigitalOut(p8), DigitalOut(p11), DigitalOut(p12),
rkk 2:5914e8cbf992 32 // DigitalOut(p13), DigitalOut(p29), DigitalOut(p30), DigitalOut(p35)
rkk 2:5914e8cbf992 33 //};
rkk 2:5914e8cbf992 34
rkk 2:5914e8cbf992 35 DigitalOut lra_en[NUM_ENS] = {
rkk 2:5914e8cbf992 36 DigitalOut(p7), DigitalOut(p8),DigitalOut(p11),DigitalOut(p12),
rkk 3:d97aeeb78f1c 37 DigitalOut(p13),DigitalOut(p29),DigitalOut(p30)
rkk 1:f213d3a65eb4 38 };
rkk 0:5e236dc97961 39
rkk 0:5e236dc97961 40 int lraOn_ms[NUM_LRAS];
rkk 0:5e236dc97961 41 int lraPeriod_ms[NUM_LRAS];
rkk 0:5e236dc97961 42 float lraIntensity[NUM_LRAS];
rkk 0:5e236dc97961 43
rkk 5:10b1ff176798 44 // THREAD POINTERS
rkk 0:5e236dc97961 45 Thread* lra_thread[NUM_LRAS];
rkk 0:5e236dc97961 46 Thread* commThread;
rkk 2:5914e8cbf992 47 RtosTimer* timer;
rkk 2:5914e8cbf992 48 unsigned long counter_ms;
rkk 0:5e236dc97961 49
rkk 0:5e236dc97961 50 void timer_cb(void const *n)
rkk 0:5e236dc97961 51 {
rkk 2:5914e8cbf992 52 counter_ms++;
rkk 0:5e236dc97961 53 }
rkk 0:5e236dc97961 54
rkk 0:5e236dc97961 55 void lra_fun(void const *n)
rkk 0:5e236dc97961 56 {
rkk 2:5914e8cbf992 57 unsigned long startTime_ms;
rkk 2:5914e8cbf992 58 int elapsed_ms;
rkk 0:5e236dc97961 59 int leftToWait_ms;
rkk 0:5e236dc97961 60
rkk 0:5e236dc97961 61 while (true) {
rkk 2:5914e8cbf992 62 // Turn On LRA:
rkk 5:10b1ff176798 63 //leds[(int)n] = 1;
rkk 1:f213d3a65eb4 64 lra_en[(int)n] = 1;
rkk 2:5914e8cbf992 65 lra[(int)n] = lraIntensity[(int)n]; //set initial intensity
rkk 2:5914e8cbf992 66 startTime_ms = counter_ms; //get start time
rkk 0:5e236dc97961 67
rkk 2:5914e8cbf992 68 leftToWait_ms = lraOn_ms[(int)n];
rkk 0:5e236dc97961 69 while( leftToWait_ms > 0) {
rkk 0:5e236dc97961 70 //printf("time: %d\n",leftToWait_ms);
rkk 0:5e236dc97961 71 Thread::signal_wait(0x1,(uint32_t) leftToWait_ms); //signal number, wait time
rkk 2:5914e8cbf992 72 elapsed_ms = (int)(counter_ms-startTime_ms);
rkk 2:5914e8cbf992 73 leftToWait_ms = lraOn_ms[(int)n] - elapsed_ms;
rkk 2:5914e8cbf992 74 lra[(int)n] = lraIntensity[(int)n]; //adjust intensity according to current value
rkk 0:5e236dc97961 75 }
rkk 0:5e236dc97961 76
rkk 1:f213d3a65eb4 77
rkk 5:10b1ff176798 78 //leds[(int)n] = 0;
rkk 1:f213d3a65eb4 79 //Set LRA PWM to 0.5
rkk 5:10b1ff176798 80 lra[(int)n] = 0.5f; // that turns off the motor!
rkk 2:5914e8cbf992 81 //Turn LRA Off by setting enable pin to 0
rkk 5:10b1ff176798 82 lra_en[(int)n] = 0; // no braking happening
rkk 0:5e236dc97961 83
rkk 0:5e236dc97961 84 //set rest time to sleep while the lra is off
rkk 2:5914e8cbf992 85 elapsed_ms = (int)(counter_ms-startTime_ms);
rkk 2:5914e8cbf992 86 leftToWait_ms = lraPeriod_ms[(int)n]-elapsed_ms;
rkk 0:5e236dc97961 87 while( leftToWait_ms > 0) {
rkk 0:5e236dc97961 88 //printf("time: %d\n",leftToWait_ms);
rkk 0:5e236dc97961 89 Thread::signal_wait(0x2,(uint32_t) leftToWait_ms); //signal number, wait time
rkk 0:5e236dc97961 90 //it woke up!
rkk 2:5914e8cbf992 91 elapsed_ms = (int)(counter_ms-startTime_ms);
rkk 2:5914e8cbf992 92 leftToWait_ms = lraPeriod_ms[(int)n] - elapsed_ms;;
rkk 0:5e236dc97961 93 }
rkk 0:5e236dc97961 94 }
rkk 0:5e236dc97961 95 }
rkk 0:5e236dc97961 96
rkk 0:5e236dc97961 97 // Called everytime a new character goes into
rkk 0:5e236dc97961 98 // the RX buffer. Test that character for \n
rkk 0:5e236dc97961 99 // Note, rxGetLastChar() gets the last char that
rkk 0:5e236dc97961 100 // we received but it does NOT remove it from
rkk 0:5e236dc97961 101 // the RX buffer.
rkk 0:5e236dc97961 102 void rx_cb(MODSERIAL_IRQ_INFO *q)
rkk 0:5e236dc97961 103 {
rkk 0:5e236dc97961 104 MODSERIAL *serial = q->serial;
rkk 0:5e236dc97961 105 if ( serial->rxGetLastChar() == '\0') {
rkk 0:5e236dc97961 106 commThread->signal_set(0x3); //signal 3
rkk 0:5e236dc97961 107 }
rkk 0:5e236dc97961 108 }
rkk 0:5e236dc97961 109
rkk 0:5e236dc97961 110 void commThread_cb(void const *n)
rkk 0:5e236dc97961 111 {
rkk 0:5e236dc97961 112 int index = 0;
rkk 0:5e236dc97961 113 int which = 0;
rkk 0:5e236dc97961 114 char input = 0;
rkk 0:5e236dc97961 115 float newIntensity;
rkk 0:5e236dc97961 116 int newOnTime;
rkk 0:5e236dc97961 117 int newTotalTime;
rkk 2:5914e8cbf992 118
rkk 5:10b1ff176798 119 // add calculations of start length and total period
rkk 5:10b1ff176798 120 //%FORMULA is
rkk 5:10b1ff176798 121 //%(input-c)/d
rkk 5:10b1ff176798 122 //
rkk 5:10b1ff176798 123 //
rkk 5:10b1ff176798 124 //% start length
rkk 5:10b1ff176798 125 //a = 1; %character start value
rkk 5:10b1ff176798 126 //b = 255; %character end value
rkk 5:10b1ff176798 127 //u = 50; %start value mapped to this lower limit
rkk 5:10b1ff176798 128 //t = 300; %end value mapped to this upper limit
rkk 5:10b1ff176798 129 //
rkk 5:10b1ff176798 130 //c=(a-u*b/t)/(1-u/t)
rkk 5:10b1ff176798 131 //d = (b-c)/t
rkk 5:10b1ff176798 132 //
rkk 5:10b1ff176798 133 //
rkk 5:10b1ff176798 134 //%%%%%%%%%%%%%%%%%%%%DIFFERENT CALCULATION
rkk 5:10b1ff176798 135 //
rkk 5:10b1ff176798 136 //% total length
rkk 5:10b1ff176798 137 //u = 300;
rkk 5:10b1ff176798 138 //t = 4000;
rkk 5:10b1ff176798 139 //c=(a-u*b/t)/(1-u/t)
rkk 5:10b1ff176798 140 //d = (b-c)/t
rkk 5:10b1ff176798 141
rkk 0:5e236dc97961 142 while (true) {
rkk 0:5e236dc97961 143 // Wait here until we detect the \0 going into the buffer.
rkk 0:5e236dc97961 144 //new data in the buffer, read it out
rkk 0:5e236dc97961 145 while(bt.readable()) {
rkk 0:5e236dc97961 146 input = bt.getc();
rkk 0:5e236dc97961 147 switch ( index ) {
rkk 0:5e236dc97961 148 case 0:
rkk 0:5e236dc97961 149 which = input-'0';
rkk 0:5e236dc97961 150 index = (which < 0)? 4 : index;
rkk 0:5e236dc97961 151 index = (which > (NUM_LRAS-1))? 4 : index;
rkk 0:5e236dc97961 152 break;
rkk 0:5e236dc97961 153 case 1:
rkk 0:5e236dc97961 154 // Intensity
rkk 0:5e236dc97961 155 input = (input < 1)? 1 : input;
rkk 0:5e236dc97961 156 input = (input > 255)? 255 : input;
rkk 0:5e236dc97961 157 // scale intensity between 0.5f to 1.0f
rkk 0:5e236dc97961 158 newIntensity = (float) (input+253)/508.0;
rkk 0:5e236dc97961 159 lraIntensity[which] = newIntensity;
rkk 0:5e236dc97961 160 break;
rkk 0:5e236dc97961 161 case 2:
rkk 0:5e236dc97961 162 // Period Length Start
rkk 0:5e236dc97961 163 input = (input < 1)? 1 : input;
rkk 0:5e236dc97961 164 input = (input > 255)? 255 : input;
rkk 0:5e236dc97961 165 // scale start length between 50 to 300 - see matlab script "range_calculations.m" in git repo
rkk 0:5e236dc97961 166 newOnTime = (int) floor( ((input+49.8)/1.016) + 0.5); //floor(...+0.5) = round()
rkk 0:5e236dc97961 167 if(newOnTime!=lraOn_ms[which]) {
rkk 0:5e236dc97961 168 lraOn_ms[which] = newOnTime;
rkk 0:5e236dc97961 169 lra_thread[which]->signal_set(0x1); //signal 1
rkk 0:5e236dc97961 170 }
rkk 0:5e236dc97961 171 case 3:
rkk 0:5e236dc97961 172 // Total Period Length
rkk 0:5e236dc97961 173 input = (input < 1)? 1 : input;
rkk 0:5e236dc97961 174 input = (input > 255)? 255 : input;
rkk 5:10b1ff176798 175 // scale total period length between 300 to 4000 - see matlab script "range_calculations.m" in git repo
rkk 0:5e236dc97961 176 newTotalTime = (int) floor( ((input+19.5946)/0.0686) +0.5); //floor(...+0.5) = round()
rkk 0:5e236dc97961 177 if(newTotalTime!=lraPeriod_ms[which]) {
rkk 0:5e236dc97961 178 lraPeriod_ms[which] = newTotalTime;
rkk 0:5e236dc97961 179 lra_thread[which]->signal_set(0x2); //signal 2
rkk 0:5e236dc97961 180 }
rkk 0:5e236dc97961 181 break;
rkk 0:5e236dc97961 182 default:
rkk 0:5e236dc97961 183 // do nothing
rkk 0:5e236dc97961 184 break;
rkk 0:5e236dc97961 185 }
rkk 0:5e236dc97961 186 index++;
rkk 5:10b1ff176798 187 //Thread::yield();// Pass control to next thread that is in state READY.
rkk 0:5e236dc97961 188 }
rkk 0:5e236dc97961 189 index = 0; //reset index
rkk 0:5e236dc97961 190 Thread::signal_wait(0x3); //signal 3
rkk 0:5e236dc97961 191 }
rkk 0:5e236dc97961 192 }
rkk 0:5e236dc97961 193 int main (void)
rkk 0:5e236dc97961 194 {
rkk 0:5e236dc97961 195 //Init communication
rkk 0:5e236dc97961 196 bt.baud(BT_BAUD); //set baud rate of bluetooth connection
rkk 0:5e236dc97961 197 bt.attach(&rx_cb, MODSERIAL::RxIrq); // attach callback to get '\0' command
rkk 0:5e236dc97961 198
rkk 5:10b1ff176798 199 commThread = new Thread(commThread_cb); //Thread(commThread_cb,NULL,osPriorityBelowNormal)
rkk 2:5914e8cbf992 200 //start universal timer to count up a counter
rkk 5:10b1ff176798 201 timer = new RtosTimer(timer_cb,osTimerPeriodic); // adjust prorioty of osTimerThread ?
rkk 2:5914e8cbf992 202 counter_ms=0;
rkk 2:5914e8cbf992 203 timer->start(1); //run timer every millisecond
rkk 2:5914e8cbf992 204
rkk 2:5914e8cbf992 205 for(int i = 0; i < NUM_ENS; i++) {
rkk 2:5914e8cbf992 206 lra_en[i] = 0;
rkk 2:5914e8cbf992 207 }
rkk 2:5914e8cbf992 208
rkk 2:5914e8cbf992 209
rkk 0:5e236dc97961 210 //initialize and start everything
rkk 0:5e236dc97961 211 for(int i = 0; i < NUM_LRAS; i++) {
rkk 2:5914e8cbf992 212
rkk 1:f213d3a65eb4 213 //set pwm frequency
rkk 2:5914e8cbf992 214 lra[i].period_us(100);
rkk 2:5914e8cbf992 215
rkk 2:5914e8cbf992 216 //initialize values
rkk 2:5914e8cbf992 217 lra[i] = 0.5f;
rkk 2:5914e8cbf992 218
rkk 0:5e236dc97961 219 //set starting vibration
rkk 0:5e236dc97961 220 lraOn_ms[i] = 100;
rkk 0:5e236dc97961 221 lraPeriod_ms[i] = 1000;
rkk 2:5914e8cbf992 222 lraIntensity[i] = 0.0f;
rkk 0:5e236dc97961 223 //Set up lra threads
rkk 5:10b1ff176798 224 lra_thread[i] = new Thread(lra_fun, (void *)i); //Thread(lra_fun, (void *)i, osPriorityNormal)
rkk 0:5e236dc97961 225 }
rkk 0:5e236dc97961 226 }