Table controller for piswarm-office
Fork of PiSwarmTableController by
main.cpp
- Committer:
- hee502
- Date:
- 2014-07-18
- Revision:
- 4:3cbff30b7b7b
- Parent:
- 3:bc7a0f14b28a
- Child:
- 5:68a1ce96bfeb
File content as of revision 4:3cbff30b7b7b:
//TABLE CONTROLLER //-Reads Sensors //-Decides table target position & movement speed //-Sends data via RF #include "mbed.h" #include "main.h" #include "communications.h" #include "display.h" //Display driver for the Midas 16x2 I2C Display //OBJECTS PwmOut ir_pwm_out(p21); //PWM Output for the IR LED driver DigitalOut ir_led(LED1); DigitalOut tx_led(LED3); DigitalOut pir_led(LED4); AnalogIn input_1(p20); AnalogIn input_2(p19); DigitalIn input_3(p18); DigitalIn input_4(p17); DigitalIn input_5(p16); DigitalIn input_6(p15); Serial pc (USBTX, USBRX); Display display; Alpha433 rf; Timer ir_led_timer; //System timer is used for timer the on-off periods for the LEDs Timer command_timer; //Timer for sending rf messages Ticker polling_ticker; //Ticker for polling the input sensors Ticker sound_ticker; //VARIABLES int off_period = 975000; //Off-period for the IR LEDs in microseconds int on_period = 25000; //On-period for the IR LEDs in microseconds 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] char use_ir_leds = 1; //Set to 0 to disable IR LEDs, 1 to enable char input_3_active = 0; char input_4_active = 0; char input_5_active = 0; char input_6_active = 0; int input_3_window = 0; int input_4_window = 0; int input_5_window = 0; int input_6_window = 0; float polling_rate = 0.1; // Period in seconds to poll sensors int polling_decay = 300; // Number of polls to decrement counters int polling_count = 0; float speed = 0; int sound = 0; float soundRaw = 0; float soundFiltered = 0; float soundFilteredPrev = 0; float targetX = 0; float targetY = 0; char data[13];//0-3:speed, 4:sound, 5-8:targetX, 9-12:targetY char * dataP; int level_3 = 0; int level_4 = 0; int level_5 = 0; int level_6 = 0; //int counter = 0;//for Edgar to try effect of sound change //PROTOTYPES void broadcast_user_rf_command(int function, char * message, int length); int get_output_power(); void polling(); void soundReading(); int main() { init(); char phase = 0; char motor_step = 0; ir_led_timer.start(); ir_pwm_out.period_us(1000); ir_pwm_out.pulsewidth_us(0); polling_ticker.attach(&polling,0.2); soundFiltered = 3400.00;//R17=137K, background~3400 sound_ticker.attach(&soundReading,0.01); while(1) { if(command_timer.read_us() > 500000){ command_timer.reset(); motor_step ++; if(motor_step > 4) motor_step = 0; switch(motor_step){ case 0: broadcast_user_rf_command(0,"Hello",5);break; case 1: broadcast_user_rf_command(1,"there",5);break; case 2: broadcast_user_rf_command(2,"you",3);break; case 3: broadcast_user_rf_command(3,"munters",7);break; } } if(phase==0){//IRs OFF if(ir_led_timer.read_us() >= off_period){ ir_led_timer.reset(); int pw = get_output_power(); if(use_ir_leds) ir_pwm_out.pulsewidth_us(pw); ir_led=1; phase = 1; } }else{//IRs ON if(ir_led_timer.read_us() >= on_period){ ir_led_timer.reset(); ir_pwm_out.pulsewidth_us(0); ir_led=0; phase = 0; } } } } void broadcast_user_rf_command(int function, char * message, int length) { //This function augments the communications stack //It sends a 'user' RF command to all members (ie target_id = 0) //It sends a 'request', not a 'command', meaning it will still be handled if commands are disabled (RF_ALLOW_COMMANDS set to 0, recommended) //It takes three inputs: // * function (an integer from 0 to 15) // * message (a char array) // * length (length of message in bytes) send_rf_message(0,48+(function % 16),message,length); } int get_output_power(){ switch(power){ case 1: return 500; case 2: return 750; case 3: return 1000; } return 250; } void polling(){ /*Movement*/ if(input_3.read() == 1){ if(level_3 < 10){ level_3++; } } else{ if(level_3 > 0){ level_3--; } } if(input_4.read() == 1){ if(level_4 < 10){ level_4++; } } else{ if(level_4 > 0){ level_4--; } } if(input_5.read() == 1){ if(level_5 < 10){ level_5++; } } else{ if(level_5 > 0){ level_5--; } } speed = level_3 + level_4 + level_5; speed /= 3.0; speed /= 10.0; speed *= 0.15; speed += 0.05; targetX = 0.5 - level_4 * 0.05; targetY = 0.5 + level_3 * 0.05; targetY = 0.5 - level_5 * 0.05; snprintf(data,13,"%0.2f%c%0.2f%0.2f", speed, sound, targetX, targetY); char * dataP = &data[0]; broadcast_user_rf_command(0, dataP, 13); display.clear_display(); display.write_string(dataP,14); /*Sound*/ snprintf(data,15,"%0.2f %d",soundFiltered,sound); display.set_position(1,0);//2nd line display.write_string(dataP,16); /* polling_count ++; char pir_active = 0; char state_changed = 0; if(input_3.read()==1){ pir_active = 1; if(input_3_active == 0){ state_changed = 1; input_3_active = 1; input_3_window ++; } }else { input_3_active = 0; if(polling_count == polling_decay){ input_3_window --; if(input_3_window < 0) input_3_window = 0; } } if(input_4.read()==1){ pir_active = 1; if(input_4_active == 0){ state_changed = 1; input_4_active = 1; input_4_window ++; } }else { input_4_active = 0; if(polling_count == polling_decay){ input_4_window --; if(input_4_window < 0) input_4_window = 0; } } if(input_5.read()==1){ pir_active = 1; if(input_5_active == 0){ state_changed = 1; input_5_active = 1; input_5_window ++; } }else { input_5_active = 0; if(polling_count == polling_decay){ input_5_window --; if(input_5_window < 0) input_5_window = 0; } } if(input_6.read()==1){ pir_active = 1; if(input_6_active == 0){ state_changed = 1; input_6_active = 1; input_6_window ++; } }else { input_6_active = 0; if(polling_count == polling_decay){ input_6_window --; if(input_6_window < 0) input_6_window = 0; } } pir_led=pir_active; if(polling_count == polling_decay) { polling_count = 0; state_changed = 1; } if(state_changed == 1){ display.clear_display(); display.set_position(0,0); char text [17]; snprintf(text,17,"3:%i 4:%i 5:%i 6:%i",input_3_window,input_4_window,input_5_window,input_6_window); display.write_string(text,16); }*/ } void soundReading(void){ soundFilteredPrev = soundFiltered;//save previous value sound = input_1.read_u16();//read new value soundFiltered *= 0.96;//filter new value soundRaw = sound * 0.04; soundFiltered += soundRaw; /*if((soundFiltered - soundFilteredPrev) > 10.00) soundFiltered = soundFilteredPrev + 10.00; if((soundFiltered - soundFilteredPrev) < -10.00) soundFiltered = soundFilteredPrev - 10.00;*/ } void handleUserRFCommand(char sender, char broadcast_message, char request_response, char id, char is_command, char function, char * data, char length){ } void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){ } void processRawRFData(char * rstring, char cCount){ } void handleData(char * data, char length) { display.set_position(1,1); display.write_string(data,length); } void init() { display.init_display(); display.set_position(0,2); display.write_string("YORK ROBOTICS",13); display.set_position(1,3); display.write_string("LABORATORY",10); wait(0.45); display.clear_display(); display.set_position(0,1); display.write_string("Pi Swarm Table",14); display.set_position(1,3); display.write_string("Controller",10); wait(0.45); rf.rf_init(); rf.setFrequency(435000000); rf.setDatarate(57600); }