sensory_array / Mbed 2 deprecated ymbed_haptic_belt_slave

Dependencies:   MODSERIAL mbed-rtos mbed

main.cpp@1:f213d3a65eb4, 2015-03-21 (annotated)

Committer:
rkk
Date:
Sat Mar 21 20:47:36 2015 +0000
Revision:
1:f213d3a65eb4
Parent:
0:5e236dc97961
Child:
2:5914e8cbf992
added motors, so it can be used

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