Revision 4:3cbff30b7b7b, committed 2014-07-18

Comitter:

hee502

Date:

Fri Jul 18 21:23:37 2014 +0000

Parent:

3:bc7a0f14b28a

Commit message:

Meme project code for table controller

Changed in this revision

--- a/main.cpp	Tue Jun 10 11:08:58 2014 +0000
+++ b/main.cpp	Fri Jul 18 21:23:37 2014 +0000
@@ -1,21 +1,14 @@
-/* University of York Robot Lab
- *
- * Pi Swarm Table Controller Demo Code
- *
- * This file is intended for use exclusively with the Pi Swarm Table Controller (PCB 1.0)
- * 
- * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
- *
- * May 2014
- *
- */
+//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);
@@ -27,32 +20,95 @@
 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 
@@ -76,7 +132,58 @@
 }
 
 void polling(){
-    polling_count ++;
+    /*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){
@@ -150,53 +257,21 @@
         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);
-    }
+    }*/
 }
 
-int main()
-{
-    init();
-    char phase = 0;
-    char motor_step = 0;
-    ir_led_timer.start();
-    //command_timer.start();
-    ir_pwm_out.period_us(1000);
-    ir_pwm_out.pulsewidth_us(0);
-    polling_ticker.attach(&polling,0.1);
-    
-
-    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){
-          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{
-          if(ir_led_timer.read_us() >= on_period){
-            ir_led_timer.reset();
-            ir_pwm_out.pulsewidth_us(0);
-            ir_led=0;
-            phase = 0;
-          }   
-        }
-    }
+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){ 
 }