sensory_array / Mbed 2 deprecated ymbed_haptic_belt_slave

Dependencies:   MODSERIAL mbed-rtos mbed

main.cpp@0:5e236dc97961, 2015-03-21 (annotated)

Committer:
rkk
Date:
Sat Mar 21 19:07:04 2015 +0000
Revision:
0:5e236dc97961
Child:
1:f213d3a65eb4
first commit for the more capable slave firmware;

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 0:5e236dc97961 7 #define NUM_LRAS 4 //8
rkk 0:5e236dc97961 8
rkk 0:5e236dc97961 9 // bluetooth serial
rkk 0:5e236dc97961 10 // p9 - tx, p10 - rx
rkk 0:5e236dc97961 11 MODSERIAL bt(p9, p10); //only receiving pin is actually needed
rkk 0:5e236dc97961 12
rkk 0:5e236dc97961 13 DigitalOut lra[NUM_LRAS] = {
rkk 0:5e236dc97961 14 DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
rkk 0:5e236dc97961 15 };
rkk 0:5e236dc97961 16
rkk 0:5e236dc97961 17 //PwmOut lra[NUM_LRAS] = {
rkk 0:5e236dc97961 18 // PwmOut(p5), PwmOut(p6), PwmOut(p14), PwmOut(p20),
rkk 0:5e236dc97961 19 // PwmOut(p25), PwmOut(p26), PwmOut(p34), PwmOut(p36)
rkk 0:5e236dc97961 20 //};
rkk 0:5e236dc97961 21
rkk 0:5e236dc97961 22 //DigitalOut lra_en[NUM_LRAS] = {
rkk 0:5e236dc97961 23 // DigitalOut(p7), DigitalOut(p8), DigitalOut(p11), DigitalOut(p12),
rkk 0:5e236dc97961 24 // DigitalOut(p13), DigitalOut(p29), DigitalOut(p30), DigitalOut(p35)
rkk 0:5e236dc97961 25 //};
rkk 0:5e236dc97961 26
rkk 0:5e236dc97961 27 int lraOn_ms[NUM_LRAS];
rkk 0:5e236dc97961 28 int lraPeriod_ms[NUM_LRAS];
rkk 0:5e236dc97961 29 float lraIntensity[NUM_LRAS];
rkk 0:5e236dc97961 30
rkk 0:5e236dc97961 31 Thread* lra_thread[NUM_LRAS];
rkk 0:5e236dc97961 32 Thread* commThread;
rkk 0:5e236dc97961 33 RtosTimer* timer[NUM_LRAS];
rkk 0:5e236dc97961 34 int counter[NUM_LRAS] = {0};
rkk 0:5e236dc97961 35
rkk 0:5e236dc97961 36 void timer_cb(void const *n)
rkk 0:5e236dc97961 37 {
rkk 0:5e236dc97961 38 counter[(int)n]++;
rkk 0:5e236dc97961 39 }
rkk 0:5e236dc97961 40
rkk 0:5e236dc97961 41 void lra_fun(void const *n)
rkk 0:5e236dc97961 42 {
rkk 0:5e236dc97961 43 int leftToWait_ms;
rkk 0:5e236dc97961 44
rkk 0:5e236dc97961 45 while (true) {
rkk 0:5e236dc97961 46 // Turn On LRA:
rkk 0:5e236dc97961 47 //lra_en[(int)n] = 1;
rkk 0:5e236dc97961 48 lra[(int)n] = ceil(lraIntensity[(int)n]); //set initial intensity
rkk 0:5e236dc97961 49 counter[(int)n] = 0; //reset counter to 0
rkk 0:5e236dc97961 50
rkk 0:5e236dc97961 51 leftToWait_ms = lraOn_ms[(int)n] - counter[(int)n];
rkk 0:5e236dc97961 52 while( leftToWait_ms > 0) {
rkk 0:5e236dc97961 53 //printf("time: %d\n",leftToWait_ms);
rkk 0:5e236dc97961 54 Thread::signal_wait(0x1,(uint32_t) leftToWait_ms); //signal number, wait time
rkk 0:5e236dc97961 55 leftToWait_ms = lraOn_ms[(int)n] - counter[(int)n];
rkk 0:5e236dc97961 56 lra[(int)n] = ceil(lraIntensity[(int)n]); //adjust intensity according to current value
rkk 0:5e236dc97961 57 }
rkk 0:5e236dc97961 58
rkk 0:5e236dc97961 59 //Turn LRA Off
rkk 0:5e236dc97961 60 lra[(int)n] = 0;
rkk 0:5e236dc97961 61
rkk 0:5e236dc97961 62 //set rest time to sleep while the lra is off
rkk 0:5e236dc97961 63 leftToWait_ms = lraPeriod_ms[(int)n]-counter[(int)n];
rkk 0:5e236dc97961 64 while( leftToWait_ms > 0) {
rkk 0:5e236dc97961 65 //printf("time: %d\n",leftToWait_ms);
rkk 0:5e236dc97961 66 Thread::signal_wait(0x2,(uint32_t) leftToWait_ms); //signal number, wait time
rkk 0:5e236dc97961 67 //it woke up!
rkk 0:5e236dc97961 68 leftToWait_ms = lraPeriod_ms[(int)n] - counter[(int)n];
rkk 0:5e236dc97961 69 }
rkk 0:5e236dc97961 70 }
rkk 0:5e236dc97961 71 }
rkk 0:5e236dc97961 72
rkk 0:5e236dc97961 73 // Called everytime a new character goes into
rkk 0:5e236dc97961 74 // the RX buffer. Test that character for \n
rkk 0:5e236dc97961 75 // Note, rxGetLastChar() gets the last char that
rkk 0:5e236dc97961 76 // we received but it does NOT remove it from
rkk 0:5e236dc97961 77 // the RX buffer.
rkk 0:5e236dc97961 78 void rx_cb(MODSERIAL_IRQ_INFO *q)
rkk 0:5e236dc97961 79 {
rkk 0:5e236dc97961 80 MODSERIAL *serial = q->serial;
rkk 0:5e236dc97961 81 if ( serial->rxGetLastChar() == '\0') {
rkk 0:5e236dc97961 82 commThread->signal_set(0x3); //signal 3
rkk 0:5e236dc97961 83 }
rkk 0:5e236dc97961 84 }
rkk 0:5e236dc97961 85
rkk 0:5e236dc97961 86 void commThread_cb(void const *n)
rkk 0:5e236dc97961 87 {
rkk 0:5e236dc97961 88 int index = 0;
rkk 0:5e236dc97961 89 int which = 0;
rkk 0:5e236dc97961 90 char input = 0;
rkk 0:5e236dc97961 91 float newIntensity;
rkk 0:5e236dc97961 92 int newOnTime;
rkk 0:5e236dc97961 93 int newTotalTime;
rkk 0:5e236dc97961 94
rkk 0:5e236dc97961 95 while (true) {
rkk 0:5e236dc97961 96 // Wait here until we detect the \0 going into the buffer.
rkk 0:5e236dc97961 97 //new data in the buffer, read it out
rkk 0:5e236dc97961 98 while(bt.readable()) {
rkk 0:5e236dc97961 99 input = bt.getc();
rkk 0:5e236dc97961 100 switch ( index ) {
rkk 0:5e236dc97961 101 case 0:
rkk 0:5e236dc97961 102 which = input-'0';
rkk 0:5e236dc97961 103 index = (which < 0)? 4 : index;
rkk 0:5e236dc97961 104 index = (which > (NUM_LRAS-1))? 4 : index;
rkk 0:5e236dc97961 105 break;
rkk 0:5e236dc97961 106 case 1:
rkk 0:5e236dc97961 107 // Intensity
rkk 0:5e236dc97961 108 input = (input < 1)? 1 : input;
rkk 0:5e236dc97961 109 input = (input > 255)? 255 : input;
rkk 0:5e236dc97961 110 // scale intensity between 0.5f to 1.0f
rkk 0:5e236dc97961 111 newIntensity = (float) (input+253)/508.0;
rkk 0:5e236dc97961 112 lraIntensity[which] = newIntensity;
rkk 0:5e236dc97961 113 break;
rkk 0:5e236dc97961 114 case 2:
rkk 0:5e236dc97961 115 // Period Length Start
rkk 0:5e236dc97961 116 input = (input < 1)? 1 : input;
rkk 0:5e236dc97961 117 input = (input > 255)? 255 : input;
rkk 0:5e236dc97961 118 // scale start length between 50 to 300 - see matlab script "range_calculations.m" in git repo
rkk 0:5e236dc97961 119 newOnTime = (int) floor( ((input+49.8)/1.016) + 0.5); //floor(...+0.5) = round()
rkk 0:5e236dc97961 120 if(newOnTime!=lraOn_ms[which]) {
rkk 0:5e236dc97961 121 lraOn_ms[which] = newOnTime;
rkk 0:5e236dc97961 122 lra_thread[which]->signal_set(0x1); //signal 1
rkk 0:5e236dc97961 123 }
rkk 0:5e236dc97961 124 case 3:
rkk 0:5e236dc97961 125 // Total Period Length
rkk 0:5e236dc97961 126 input = (input < 1)? 1 : input;
rkk 0:5e236dc97961 127 input = (input > 255)? 255 : input;
rkk 0:5e236dc97961 128 // scale total period length between 301 to 4000 - see matlab script "range_calculations.m" in git repo
rkk 0:5e236dc97961 129 newTotalTime = (int) floor( ((input+19.5946)/0.0686) +0.5); //floor(...+0.5) = round()
rkk 0:5e236dc97961 130 if(newTotalTime!=lraPeriod_ms[which]) {
rkk 0:5e236dc97961 131 lraPeriod_ms[which] = newTotalTime;
rkk 0:5e236dc97961 132 lra_thread[which]->signal_set(0x2); //signal 2
rkk 0:5e236dc97961 133 }
rkk 0:5e236dc97961 134 break;
rkk 0:5e236dc97961 135 default:
rkk 0:5e236dc97961 136 // do nothing
rkk 0:5e236dc97961 137 break;
rkk 0:5e236dc97961 138 }
rkk 0:5e236dc97961 139 index++;
rkk 0:5e236dc97961 140 }
rkk 0:5e236dc97961 141 index = 0; //reset index
rkk 0:5e236dc97961 142 Thread::signal_wait(0x3); //signal 3
rkk 0:5e236dc97961 143 }
rkk 0:5e236dc97961 144 }
rkk 0:5e236dc97961 145 int main (void)
rkk 0:5e236dc97961 146 {
rkk 0:5e236dc97961 147 //Init communication
rkk 0:5e236dc97961 148 bt.baud(BT_BAUD); //set baud rate of bluetooth connection
rkk 0:5e236dc97961 149 bt.attach(&rx_cb, MODSERIAL::RxIrq); // attach callback to get '\0' command
rkk 0:5e236dc97961 150
rkk 0:5e236dc97961 151 commThread = new Thread(commThread_cb);
rkk 0:5e236dc97961 152
rkk 0:5e236dc97961 153 //initialize and start everything
rkk 0:5e236dc97961 154 for(int i = 0; i < NUM_LRAS; i++) {
rkk 0:5e236dc97961 155 //set starting vibration
rkk 0:5e236dc97961 156 lraOn_ms[i] = 100;
rkk 0:5e236dc97961 157 lraPeriod_ms[i] = 1000;
rkk 0:5e236dc97961 158 lraIntensity[i] = 1;
rkk 0:5e236dc97961 159 //start timers
rkk 0:5e236dc97961 160 timer[i]= new RtosTimer(timer_cb,osTimerPeriodic, (void *) i);
rkk 0:5e236dc97961 161 timer[i]->start(1); //run timer every millisecond
rkk 0:5e236dc97961 162 //Set up lra threads
rkk 0:5e236dc97961 163 lra_thread[i] = new Thread(lra_fun, (void *)i);
rkk 0:5e236dc97961 164 }
rkk 0:5e236dc97961 165 }