sensory_array / Mbed 2 deprecated ymbed_haptic_belt_slave

Dependencies:   MODSERIAL mbed-rtos mbed

main.cpp@6:3ae957a6437b, 2015-03-24 (annotated)

Committer:
baraki
Date:
Tue Mar 24 00:14:20 2015 +0000
Revision:
6:3ae957a6437b
Parent:
5:10b1ff176798 
added #define for pwms and enables

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