Huseyin Erdem / Mbed 2 deprecated PiSwarmTableController

Table controller for piswarm-office

Dependencies:   mbed

Fork of PiSwarmTableController by piswarm

main.cpp@4:3cbff30b7b7b, 2014-07-18 (annotated)

Committer:
hee502
Date:
Fri Jul 18 21:23:37 2014 +0000
Revision:
4:3cbff30b7b7b
Parent:
3:bc7a0f14b28a
Child:
5:68a1ce96bfeb
Meme project code for table controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hee502 4:3cbff30b7b7b 1 //TABLE CONTROLLER
hee502 4:3cbff30b7b7b 2 //-Reads Sensors
hee502 4:3cbff30b7b7b 3 //-Decides table target position & movement speed
hee502 4:3cbff30b7b7b 4 //-Sends data via RF
jah128 0:d88fd55a27a6 5
jah128 0:d88fd55a27a6 6 #include "mbed.h"
jah128 2:c81f4ef63132 7 #include "main.h"
jah128 2:c81f4ef63132 8 #include "communications.h"
jah128 0:d88fd55a27a6 9 #include "display.h" //Display driver for the Midas 16x2 I2C Display
jah128 0:d88fd55a27a6 10
hee502 4:3cbff30b7b7b 11 //OBJECTS
jah128 1:e4a0d424ac8d 12 PwmOut ir_pwm_out(p21); //PWM Output for the IR LED driver
jah128 1:e4a0d424ac8d 13 DigitalOut ir_led(LED1);
jah128 2:c81f4ef63132 14 DigitalOut tx_led(LED3);
jah128 1:e4a0d424ac8d 15 DigitalOut pir_led(LED4);
jah128 1:e4a0d424ac8d 16 AnalogIn input_1(p20);
jah128 1:e4a0d424ac8d 17 AnalogIn input_2(p19);
jah128 1:e4a0d424ac8d 18 DigitalIn input_3(p18);
jah128 1:e4a0d424ac8d 19 DigitalIn input_4(p17);
jah128 1:e4a0d424ac8d 20 DigitalIn input_5(p16);
jah128 1:e4a0d424ac8d 21 DigitalIn input_6(p15);
jah128 2:c81f4ef63132 22 Serial pc (USBTX, USBRX);
jah128 0:d88fd55a27a6 23 Display display;
jah128 2:c81f4ef63132 24 Alpha433 rf;
jah128 3:bc7a0f14b28a 25 Timer ir_led_timer; //System timer is used for timer the on-off periods for the LEDs
jah128 3:bc7a0f14b28a 26 Timer command_timer; //Timer for sending rf messages
jah128 1:e4a0d424ac8d 27 Ticker polling_ticker; //Ticker for polling the input sensors
hee502 4:3cbff30b7b7b 28 Ticker sound_ticker;
jah128 0:d88fd55a27a6 29
hee502 4:3cbff30b7b7b 30 //VARIABLES
jah128 3:bc7a0f14b28a 31 int off_period = 975000; //Off-period for the IR LEDs in microseconds
jah128 3:bc7a0f14b28a 32 int on_period = 25000; //On-period for the IR LEDs in microseconds
jah128 2:c81f4ef63132 33 char power = 1; //Output power for the IR LEDs : 0=25%, 1=50%, 2=75%, 3=100% (700mA) [NB The LEDs are rated 20mA and are in parallel runs of 20, so a maximum power of 50% is recommended for long term use]
jah128 2:c81f4ef63132 34 char use_ir_leds = 1; //Set to 0 to disable IR LEDs, 1 to enable
jah128 2:c81f4ef63132 35 char input_3_active = 0;
jah128 2:c81f4ef63132 36 char input_4_active = 0;
jah128 2:c81f4ef63132 37 char input_5_active = 0;
jah128 2:c81f4ef63132 38 char input_6_active = 0;
jah128 2:c81f4ef63132 39 int input_3_window = 0;
jah128 2:c81f4ef63132 40 int input_4_window = 0;
jah128 2:c81f4ef63132 41 int input_5_window = 0;
jah128 2:c81f4ef63132 42 int input_6_window = 0;
jah128 2:c81f4ef63132 43 float polling_rate = 0.1; // Period in seconds to poll sensors
jah128 2:c81f4ef63132 44 int polling_decay = 300; // Number of polls to decrement counters
jah128 2:c81f4ef63132 45 int polling_count = 0;
jah128 0:d88fd55a27a6 46
hee502 4:3cbff30b7b7b 47 float speed = 0;
hee502 4:3cbff30b7b7b 48 int sound = 0;
hee502 4:3cbff30b7b7b 49 float soundRaw = 0;
hee502 4:3cbff30b7b7b 50 float soundFiltered = 0;
hee502 4:3cbff30b7b7b 51 float soundFilteredPrev = 0;
hee502 4:3cbff30b7b7b 52 float targetX = 0;
hee502 4:3cbff30b7b7b 53 float targetY = 0;
hee502 4:3cbff30b7b7b 54 char data[13];//0-3:speed, 4:sound, 5-8:targetX, 9-12:targetY
hee502 4:3cbff30b7b7b 55 char * dataP;
hee502 4:3cbff30b7b7b 56 int level_3 = 0;
hee502 4:3cbff30b7b7b 57 int level_4 = 0;
hee502 4:3cbff30b7b7b 58 int level_5 = 0;
hee502 4:3cbff30b7b7b 59 int level_6 = 0;
hee502 4:3cbff30b7b7b 60
hee502 4:3cbff30b7b7b 61 //int counter = 0;//for Edgar to try effect of sound change
hee502 4:3cbff30b7b7b 62
hee502 4:3cbff30b7b7b 63 //PROTOTYPES
hee502 4:3cbff30b7b7b 64 void broadcast_user_rf_command(int function, char * message, int length);
hee502 4:3cbff30b7b7b 65 int get_output_power();
hee502 4:3cbff30b7b7b 66 void polling();
hee502 4:3cbff30b7b7b 67 void soundReading();
hee502 4:3cbff30b7b7b 68
hee502 4:3cbff30b7b7b 69 int main()
hee502 4:3cbff30b7b7b 70 {
hee502 4:3cbff30b7b7b 71 init();
hee502 4:3cbff30b7b7b 72 char phase = 0;
hee502 4:3cbff30b7b7b 73 char motor_step = 0;
hee502 4:3cbff30b7b7b 74 ir_led_timer.start();
hee502 4:3cbff30b7b7b 75 ir_pwm_out.period_us(1000);
hee502 4:3cbff30b7b7b 76 ir_pwm_out.pulsewidth_us(0);
hee502 4:3cbff30b7b7b 77 polling_ticker.attach(&polling,0.2);
hee502 4:3cbff30b7b7b 78 soundFiltered = 3400.00;//R17=137K, background~3400
hee502 4:3cbff30b7b7b 79 sound_ticker.attach(&soundReading,0.01);
hee502 4:3cbff30b7b7b 80
hee502 4:3cbff30b7b7b 81 while(1) {
hee502 4:3cbff30b7b7b 82 if(command_timer.read_us() > 500000){
hee502 4:3cbff30b7b7b 83 command_timer.reset();
hee502 4:3cbff30b7b7b 84 motor_step ++;
hee502 4:3cbff30b7b7b 85 if(motor_step > 4) motor_step = 0;
hee502 4:3cbff30b7b7b 86 switch(motor_step){
hee502 4:3cbff30b7b7b 87 case 0: broadcast_user_rf_command(0,"Hello",5);break;
hee502 4:3cbff30b7b7b 88 case 1: broadcast_user_rf_command(1,"there",5);break;
hee502 4:3cbff30b7b7b 89 case 2: broadcast_user_rf_command(2,"you",3);break;
hee502 4:3cbff30b7b7b 90 case 3: broadcast_user_rf_command(3,"munters",7);break;
hee502 4:3cbff30b7b7b 91 }
hee502 4:3cbff30b7b7b 92 }
hee502 4:3cbff30b7b7b 93 if(phase==0){//IRs OFF
hee502 4:3cbff30b7b7b 94 if(ir_led_timer.read_us() >= off_period){
hee502 4:3cbff30b7b7b 95 ir_led_timer.reset();
hee502 4:3cbff30b7b7b 96 int pw = get_output_power();
hee502 4:3cbff30b7b7b 97 if(use_ir_leds) ir_pwm_out.pulsewidth_us(pw);
hee502 4:3cbff30b7b7b 98 ir_led=1;
hee502 4:3cbff30b7b7b 99 phase = 1;
hee502 4:3cbff30b7b7b 100 }
hee502 4:3cbff30b7b7b 101 }else{//IRs ON
hee502 4:3cbff30b7b7b 102 if(ir_led_timer.read_us() >= on_period){
hee502 4:3cbff30b7b7b 103 ir_led_timer.reset();
hee502 4:3cbff30b7b7b 104 ir_pwm_out.pulsewidth_us(0);
hee502 4:3cbff30b7b7b 105 ir_led=0;
hee502 4:3cbff30b7b7b 106 phase = 0;
hee502 4:3cbff30b7b7b 107 }
hee502 4:3cbff30b7b7b 108 }
hee502 4:3cbff30b7b7b 109 }
hee502 4:3cbff30b7b7b 110 }
hee502 4:3cbff30b7b7b 111
jah128 3:bc7a0f14b28a 112 void broadcast_user_rf_command(int function, char * message, int length)
jah128 0:d88fd55a27a6 113 {
jah128 3:bc7a0f14b28a 114 //This function augments the communications stack
jah128 3:bc7a0f14b28a 115 //It sends a 'user' RF command to all members (ie target_id = 0)
jah128 3:bc7a0f14b28a 116 //It sends a 'request', not a 'command', meaning it will still be handled if commands are disabled (RF_ALLOW_COMMANDS set to 0, recommended)
jah128 3:bc7a0f14b28a 117 //It takes three inputs:
jah128 3:bc7a0f14b28a 118 // * function (an integer from 0 to 15)
jah128 3:bc7a0f14b28a 119 // * message (a char array)
jah128 3:bc7a0f14b28a 120 // * length (length of message in bytes)
jah128 3:bc7a0f14b28a 121 send_rf_message(0,48+(function % 16),message,length);
jah128 0:d88fd55a27a6 122 }
jah128 3:bc7a0f14b28a 123
jah128 0:d88fd55a27a6 124
jah128 1:e4a0d424ac8d 125 int get_output_power(){
jah128 1:e4a0d424ac8d 126 switch(power){
jah128 1:e4a0d424ac8d 127 case 1: return 500;
jah128 1:e4a0d424ac8d 128 case 2: return 750;
jah128 1:e4a0d424ac8d 129 case 3: return 1000;
jah128 1:e4a0d424ac8d 130 }
jah128 1:e4a0d424ac8d 131 return 250;
jah128 0:d88fd55a27a6 132 }
jah128 0:d88fd55a27a6 133
jah128 1:e4a0d424ac8d 134 void polling(){
hee502 4:3cbff30b7b7b 135 /*Movement*/
hee502 4:3cbff30b7b7b 136 if(input_3.read() == 1){
hee502 4:3cbff30b7b7b 137 if(level_3 < 10){
hee502 4:3cbff30b7b7b 138 level_3++;
hee502 4:3cbff30b7b7b 139 }
hee502 4:3cbff30b7b7b 140 }
hee502 4:3cbff30b7b7b 141 else{
hee502 4:3cbff30b7b7b 142 if(level_3 > 0){
hee502 4:3cbff30b7b7b 143 level_3--;
hee502 4:3cbff30b7b7b 144 }
hee502 4:3cbff30b7b7b 145 }
hee502 4:3cbff30b7b7b 146 if(input_4.read() == 1){
hee502 4:3cbff30b7b7b 147 if(level_4 < 10){
hee502 4:3cbff30b7b7b 148 level_4++;
hee502 4:3cbff30b7b7b 149 }
hee502 4:3cbff30b7b7b 150 }
hee502 4:3cbff30b7b7b 151 else{
hee502 4:3cbff30b7b7b 152 if(level_4 > 0){
hee502 4:3cbff30b7b7b 153 level_4--;
hee502 4:3cbff30b7b7b 154 }
hee502 4:3cbff30b7b7b 155 }
hee502 4:3cbff30b7b7b 156 if(input_5.read() == 1){
hee502 4:3cbff30b7b7b 157 if(level_5 < 10){
hee502 4:3cbff30b7b7b 158 level_5++;
hee502 4:3cbff30b7b7b 159 }
hee502 4:3cbff30b7b7b 160 }
hee502 4:3cbff30b7b7b 161 else{
hee502 4:3cbff30b7b7b 162 if(level_5 > 0){
hee502 4:3cbff30b7b7b 163 level_5--;
hee502 4:3cbff30b7b7b 164 }
hee502 4:3cbff30b7b7b 165 }
hee502 4:3cbff30b7b7b 166 speed = level_3 + level_4 + level_5;
hee502 4:3cbff30b7b7b 167 speed /= 3.0;
hee502 4:3cbff30b7b7b 168 speed /= 10.0;
hee502 4:3cbff30b7b7b 169 speed *= 0.15;
hee502 4:3cbff30b7b7b 170 speed += 0.05;
hee502 4:3cbff30b7b7b 171 targetX = 0.5 - level_4 * 0.05;
hee502 4:3cbff30b7b7b 172 targetY = 0.5 + level_3 * 0.05;
hee502 4:3cbff30b7b7b 173 targetY = 0.5 - level_5 * 0.05;
hee502 4:3cbff30b7b7b 174
hee502 4:3cbff30b7b7b 175 snprintf(data,13,"%0.2f%c%0.2f%0.2f", speed, sound, targetX, targetY);
hee502 4:3cbff30b7b7b 176 char * dataP = &data[0];
hee502 4:3cbff30b7b7b 177 broadcast_user_rf_command(0, dataP, 13);
hee502 4:3cbff30b7b7b 178 display.clear_display();
hee502 4:3cbff30b7b7b 179 display.write_string(dataP,14);
hee502 4:3cbff30b7b7b 180
hee502 4:3cbff30b7b7b 181 /*Sound*/
hee502 4:3cbff30b7b7b 182 snprintf(data,15,"%0.2f %d",soundFiltered,sound);
hee502 4:3cbff30b7b7b 183 display.set_position(1,0);//2nd line
hee502 4:3cbff30b7b7b 184 display.write_string(dataP,16);
hee502 4:3cbff30b7b7b 185
hee502 4:3cbff30b7b7b 186 /* polling_count ++;
jah128 2:c81f4ef63132 187 char pir_active = 0;
jah128 2:c81f4ef63132 188 char state_changed = 0;
jah128 2:c81f4ef63132 189 if(input_3.read()==1){
jah128 2:c81f4ef63132 190 pir_active = 1;
jah128 2:c81f4ef63132 191 if(input_3_active == 0){
jah128 2:c81f4ef63132 192 state_changed = 1;
jah128 2:c81f4ef63132 193 input_3_active = 1;
jah128 2:c81f4ef63132 194 input_3_window ++;
jah128 2:c81f4ef63132 195 }
jah128 2:c81f4ef63132 196 }else {
jah128 2:c81f4ef63132 197 input_3_active = 0;
jah128 2:c81f4ef63132 198 if(polling_count == polling_decay){
jah128 2:c81f4ef63132 199 input_3_window --;
jah128 2:c81f4ef63132 200 if(input_3_window < 0) input_3_window = 0;
jah128 2:c81f4ef63132 201 }
jah128 2:c81f4ef63132 202 }
jah128 2:c81f4ef63132 203
jah128 2:c81f4ef63132 204 if(input_4.read()==1){
jah128 2:c81f4ef63132 205 pir_active = 1;
jah128 2:c81f4ef63132 206 if(input_4_active == 0){
jah128 2:c81f4ef63132 207 state_changed = 1;
jah128 2:c81f4ef63132 208 input_4_active = 1;
jah128 2:c81f4ef63132 209 input_4_window ++;
jah128 2:c81f4ef63132 210 }
jah128 2:c81f4ef63132 211 }else {
jah128 2:c81f4ef63132 212 input_4_active = 0;
jah128 2:c81f4ef63132 213 if(polling_count == polling_decay){
jah128 2:c81f4ef63132 214 input_4_window --;
jah128 2:c81f4ef63132 215 if(input_4_window < 0) input_4_window = 0;
jah128 2:c81f4ef63132 216 }
jah128 2:c81f4ef63132 217 }
jah128 2:c81f4ef63132 218
jah128 2:c81f4ef63132 219 if(input_5.read()==1){
jah128 2:c81f4ef63132 220 pir_active = 1;
jah128 2:c81f4ef63132 221 if(input_5_active == 0){
jah128 2:c81f4ef63132 222 state_changed = 1;
jah128 2:c81f4ef63132 223 input_5_active = 1;
jah128 2:c81f4ef63132 224 input_5_window ++;
jah128 2:c81f4ef63132 225 }
jah128 2:c81f4ef63132 226 }else {
jah128 2:c81f4ef63132 227 input_5_active = 0;
jah128 2:c81f4ef63132 228 if(polling_count == polling_decay){
jah128 2:c81f4ef63132 229 input_5_window --;
jah128 2:c81f4ef63132 230 if(input_5_window < 0) input_5_window = 0;
jah128 2:c81f4ef63132 231 }
jah128 2:c81f4ef63132 232 }
jah128 2:c81f4ef63132 233
jah128 2:c81f4ef63132 234 if(input_6.read()==1){
jah128 2:c81f4ef63132 235 pir_active = 1;
jah128 2:c81f4ef63132 236 if(input_6_active == 0){
jah128 2:c81f4ef63132 237 state_changed = 1;
jah128 2:c81f4ef63132 238 input_6_active = 1;
jah128 2:c81f4ef63132 239 input_6_window ++;
jah128 2:c81f4ef63132 240 }
jah128 2:c81f4ef63132 241 }else {
jah128 2:c81f4ef63132 242 input_6_active = 0;
jah128 2:c81f4ef63132 243 if(polling_count == polling_decay){
jah128 2:c81f4ef63132 244 input_6_window --;
jah128 2:c81f4ef63132 245 if(input_6_window < 0) input_6_window = 0;
jah128 2:c81f4ef63132 246 }
jah128 2:c81f4ef63132 247 }
jah128 2:c81f4ef63132 248
jah128 2:c81f4ef63132 249 pir_led=pir_active;
jah128 2:c81f4ef63132 250 if(polling_count == polling_decay) {
jah128 2:c81f4ef63132 251 polling_count = 0;
jah128 2:c81f4ef63132 252 state_changed = 1;
jah128 2:c81f4ef63132 253 }
jah128 2:c81f4ef63132 254 if(state_changed == 1){
jah128 2:c81f4ef63132 255 display.clear_display();
jah128 2:c81f4ef63132 256 display.set_position(0,0);
jah128 2:c81f4ef63132 257 char text [17];
jah128 2:c81f4ef63132 258 snprintf(text,17,"3:%i 4:%i 5:%i 6:%i",input_3_window,input_4_window,input_5_window,input_6_window);
jah128 2:c81f4ef63132 259 display.write_string(text,16);
hee502 4:3cbff30b7b7b 260 }*/
jah128 0:d88fd55a27a6 261 }
jah128 0:d88fd55a27a6 262
hee502 4:3cbff30b7b7b 263 void soundReading(void){
hee502 4:3cbff30b7b7b 264 soundFilteredPrev = soundFiltered;//save previous value
hee502 4:3cbff30b7b7b 265 sound = input_1.read_u16();//read new value
hee502 4:3cbff30b7b7b 266 soundFiltered *= 0.96;//filter new value
hee502 4:3cbff30b7b7b 267 soundRaw = sound * 0.04;
hee502 4:3cbff30b7b7b 268 soundFiltered += soundRaw;
hee502 4:3cbff30b7b7b 269 /*if((soundFiltered - soundFilteredPrev) > 10.00)
hee502 4:3cbff30b7b7b 270 soundFiltered = soundFilteredPrev + 10.00;
hee502 4:3cbff30b7b7b 271 if((soundFiltered - soundFilteredPrev) < -10.00)
hee502 4:3cbff30b7b7b 272 soundFiltered = soundFilteredPrev - 10.00;*/
jah128 2:c81f4ef63132 273 }
jah128 2:c81f4ef63132 274
jah128 2:c81f4ef63132 275 void handleUserRFCommand(char sender, char broadcast_message, char request_response, char id, char is_command, char function, char * data, char length){
jah128 2:c81f4ef63132 276 }
jah128 2:c81f4ef63132 277
jah128 2:c81f4ef63132 278 void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){
jah128 2:c81f4ef63132 279 }
jah128 2:c81f4ef63132 280
jah128 2:c81f4ef63132 281 void processRawRFData(char * rstring, char cCount){
jah128 2:c81f4ef63132 282 }
jah128 2:c81f4ef63132 283
jah128 2:c81f4ef63132 284
jah128 2:c81f4ef63132 285 void handleData(char * data, char length)
jah128 2:c81f4ef63132 286 {
jah128 2:c81f4ef63132 287 display.set_position(1,1);
jah128 2:c81f4ef63132 288 display.write_string(data,length);
jah128 2:c81f4ef63132 289 }
jah128 3:bc7a0f14b28a 290
jah128 3:bc7a0f14b28a 291
jah128 3:bc7a0f14b28a 292 void init()
jah128 3:bc7a0f14b28a 293 {
jah128 3:bc7a0f14b28a 294 display.init_display();
jah128 3:bc7a0f14b28a 295 display.set_position(0,2);
jah128 3:bc7a0f14b28a 296 display.write_string("YORK ROBOTICS",13);
jah128 3:bc7a0f14b28a 297 display.set_position(1,3);
jah128 3:bc7a0f14b28a 298 display.write_string("LABORATORY",10);
jah128 3:bc7a0f14b28a 299 wait(0.45);
jah128 3:bc7a0f14b28a 300 display.clear_display();
jah128 3:bc7a0f14b28a 301 display.set_position(0,1);
jah128 3:bc7a0f14b28a 302 display.write_string("Pi Swarm Table",14);
jah128 3:bc7a0f14b28a 303 display.set_position(1,3);
jah128 3:bc7a0f14b28a 304 display.write_string("Controller",10);
jah128 3:bc7a0f14b28a 305 wait(0.45);
jah128 3:bc7a0f14b28a 306 rf.rf_init();
jah128 3:bc7a0f14b28a 307 rf.setFrequency(435000000);
jah128 3:bc7a0f14b28a 308 rf.setDatarate(57600);
jah128 3:bc7a0f14b28a 309 }