Diff: main.cpp

Revision:

1:37502eb3b70f

Parent:

0:46cd1498a39a

Child:

2:e806b595f9ce

--- a/main.cpp	Mon Nov 11 00:01:15 2013 +0000
+++ b/main.cpp	Sun Feb 02 21:18:16 2014 +0000
@@ -1,199 +1,82 @@
-// York Robotics Laboratory
-// 3-Pi Swarm Robot Test Code
-// Version 0.1 - 11th November, 2013
-// James Hilder, University of York 
+/* University of York Robot Lab Pi Swarm Robot Library
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ * 
+ * Version 0.4  January 2014
+ *
+ * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.1
+ */
 
-#include "mbed.h"
-#include "s3pi.h"
+#include "main.h"   // Certain parameters can be set by changing the defines in main.h
 
-s3pi s3pi;
 
-int line_data[5];
-char shift_mode = 0;
-char current_mode = 0;
-char cycle_mode = 0;
+PiSwarm piswarm;
+Serial pc (USBTX,USBRX);
 
-void switch_pressed() {
-    //Switch pressed
-    //1 = Center
-    //2 = Right
-    //4 = Left
-    //8 = Down
-    //16 = Up
-    char switches = s3pi.get_switches();
-    //s3pi.locate(0,1);
-    //s3pi.printf("%d  ", switches);
+//This is where the program code goes.  In the simple demo, the outer LEDs are blinked.
+int main() {
+    init();
+    
+    int step = 0;
     
-    if(switches == 1) {
-        shift_mode = 1;
-        cycle_mode = 1-cycle_mode;
-    }
-    
-    if(switches == 4) {
-        shift_mode = 1;
-        if(current_mode == 0) current_mode = 17;
-        else current_mode --;
-    }
-    if(switches == 2) {
-        shift_mode = 1;
-        current_mode ++;
-        if (current_mode > 17) current_mode =0;
+    while(1) {
+        //pc.printf("%i\n",step);
+        // Do something!
+        step ++;
+        if(step==6)step=0;
+        switch (step%3){
+            case 0:  piswarm.set_oled_colour(200,0,0); break;
+            case 1:  piswarm.set_oled_colour(0,200,0); break;
+            case 2:  piswarm.set_oled_colour(0,0,200); break;
+        }
+        switch (step%2){
+            case 0:  piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0); break;
+            case 1:  piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1); break;
+        }
+        wait(0.25);
     }
 }
 
-void init() {
-    //Initialisation routine for 3-Pi Robot
-    //Displays a message on the screen and flashes the central LED
-    //Calibrates the gyro and accelerometer
-    //Make sure robot is flat and stationary when this code is run (calibration starts after about 1 second to allow robot to settle)
-    s3pi.play_tune("MSCEG>C",7);
-    s3pi.cls();
-    s3pi.enable_cled(1);
-    s3pi.set_cled_colour(200,0,0);
-    s3pi.printf("  YORK  ");
-    s3pi.locate(0,1);
-    s3pi.printf("ROBOTLAB");
-    wait(0.3);
-    s3pi.set_cled_colour(0,200,0);
-    wait(0.3);
-    s3pi.set_cled_colour(0,0,200);
-    wait(0.3);
-    s3pi.cls();
-    s3pi.set_cled_colour(20,20,20);
-    s3pi.printf("3piSwarm");
-    s3pi.locate(0,1);
-    s3pi.printf("ID : %d ",s3pi.get_id());
-    if(s3pi.calibrate_magnetometer() != 0){
-        s3pi.cls();
-        s3pi.locate(0,0);
-        s3pi.printf("Mag Cal ");
-        s3pi.locate(0,1);
-        s3pi.printf("Failed  ");
-        wait(0.5);
-    }else{
-        if(s3pi.calibrate_gyro() == 0){
-            s3pi.cls();
-            s3pi.locate(0,0);
-            s3pi.printf("Gyro Cal");
-            s3pi.locate(0,1);
-            s3pi.printf("Failed  ");
-            wait(0.5);
-        }else{
-            if(s3pi.calibrate_accelerometer() == 0){
-                s3pi.cls();
-                s3pi.locate(0,0);
-                s3pi.printf("Acc. Cal");
-                s3pi.locate(0,1);
-                s3pi.printf("Failed  ");
-                wait(0.5);
-            }
-        }
-    }
-    wait(1.5);
-    s3pi.cls();
-    s3pi.set_cled_colour(10,20,30);
-    wait(1.5);
+// Communications
+
+// If using the communication stack (USE_COMMUNICATION_STACK = 1), functionality for handling user RF responses should be added to the following functions
+// If the communication stack is not being used, all radio data is sent to processRawRFData() instead
+
+void handleUserRFCommand(char sender, char broadcast_message, char request_response, char id, char is_command, char function, char * data, char length){
+    // A 'user' RF Command has been received:  write the code here to process it
+    // sender = ID of the sender, range 0 to 31
+    // broadcast_message = 1 is message sent to all robots, 0 otherwise
+    // request_response = 1 if a response is expected, 0 otherwise
+    // id = Message ID, range 0 to 255
+    // is_command = 1 is message is a command, 0 if it is a request.  If RF_ALLOW_COMMANDS is not selected, only requests will be sent to this block
+    // function = The function identifier.  Range 0 to 15
+    // * data = Array containing extra data bytes
+    // length = Length of extra data bytes held (range 0 to 57)
+    
+}    
+
+void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){
+    // A 'user' RF Response has been received:  write the code here to process it
+    // sender = ID of the sender, range 0 to 31
+    // broadcast_message = 1 is message sent to all robots, 0 otherwise
+    // success = 1 if operation successful, 0 otherwise
+    // id = Message ID, range 0 to 255
+    // is_command = 1 is message is a command, 0 if it is a request.  If RF_ALLOW_COMMANDS is not selected, only requests will be sent to this block
+    // function = The function identifier.  Range 0 to 15
+    // * data = Array containing extra data bytes
+    // length = Length of extra data bytes held (range 0 to 57)
+  
+}    
+
+void processRawRFData(char * rstring, char cCount){
+    // A raw RF packet has been received: write the code here to process it
+    // rstring = The received packet
+    // cCount = Packet length
 }
 
-int main() {
-    init();
-    s3pi.locate(0,0);
-    s3pi.printf("  Gyro:");
-    int count = 0;
-    float sum = 0;
-    while(1) {
-        count++;
-        if(shift_mode == 1) {
-            shift_mode = 0;
-            count = 0;
-            sum = 0;
-            s3pi.cls();
-            s3pi.locate(0,0);
-            if(cycle_mode == 1) {
-                s3pi.printf("CYCLE ON");   
-            } 
-            else{
-            switch(current_mode) {
-                case 0: s3pi.printf("  Gyro: "); break;
-                case 1: s3pi.printf("  Temp: "); break;
-                case 2: s3pi.printf(" Light: "); break;
-                case 3: s3pi.printf("  AccX: "); break;
-                case 4: s3pi.printf("  AccY: "); break;
-                case 5: s3pi.printf("  AccZ: "); break;
-                case 6: s3pi.printf("  MagX: "); break;
-                case 7: s3pi.printf("  MagY: "); break;
-                case 8: s3pi.printf("  MagZ: "); break;
-                case 9: s3pi.printf(" BG IR: "); break;
-                case 10: s3pi.printf(" Dist1: "); break;
-                case 11: s3pi.printf(" Dist2: "); break;
-                case 12: s3pi.printf(" Dist3: "); break;
-                case 13: s3pi.printf(" Dist4: "); break;
-                case 14: s3pi.printf(" Dist5: "); break;
-                case 15: s3pi.printf(" Dist6: "); break;
-                case 16: s3pi.printf(" Dist7: "); break;
-                case 17: s3pi.printf(" Dist8: "); break;
-            }
-            }
-        }
-        switch(current_mode) {
-            case 0: sum+=s3pi.read_gyro(); break;
-            case 1: sum+=s3pi.read_temperature(); break;
-            case 2: sum+=s3pi.read_light_sensor(); break;
-            case 3: sum+=s3pi.read_accelerometer_x(); break;
-            case 4: sum+=s3pi.read_accelerometer_y(); break;
-            case 5: sum+=s3pi.read_accelerometer_z(); break;
-            case 6: s3pi.read_magnetometer(); sum+=s3pi.get_magnetometer_x(); break;
-            case 7: s3pi.read_magnetometer(); sum+=s3pi.get_magnetometer_y(); break;
-            case 8: s3pi.read_magnetometer(); sum+=s3pi.get_magnetometer_z(); break;
-            case 9: sum+=s3pi.read_adc_value(0)+s3pi.read_adc_value(1)+s3pi.read_adc_value(2)+s3pi.read_adc_value(3)+s3pi.read_adc_value(4)+s3pi.read_adc_value(5)+s3pi.read_adc_value(6)+s3pi.read_adc_value(7); break;
-            case 10: sum+=s3pi.read_reflected_ir_distance(0); break;
-            case 11: sum+=s3pi.read_reflected_ir_distance(1); break;
-            case 12: sum+=s3pi.read_reflected_ir_distance(2); break;
-            case 13: sum+=s3pi.read_reflected_ir_distance(3); break;
-            case 14: sum+=s3pi.read_reflected_ir_distance(4); break;      
-            case 15: sum+=s3pi.read_reflected_ir_distance(5); break;
-            case 16: sum+=s3pi.read_reflected_ir_distance(6); break;
-            case 17: sum+=s3pi.read_reflected_ir_distance(7); break;      
-        }
-        s3pi.read_raw_sensors(line_data);
-        int red = (line_data [0] - 50) / 38;
-        int green = (line_data [1] - 50) / 38;
-        int blue = (line_data [2] - 50) / 38;
-        s3pi.set_cled_colour(red,green,blue);
-        s3pi.set_oled_colour(red,green,blue);
-        if (count>9){
-            sum /= count;
-            count = 0;
-            if(cycle_mode == 1){
-                s3pi.cls();
-                s3pi.locate(0,0);
-                switch(current_mode) {
-                    case 0: s3pi.printf("  Gyro: "); break;
-                    case 1: s3pi.printf("  Temp: "); break;
-                    case 2: s3pi.printf(" Light: "); break;
-                    case 3: s3pi.printf("  AccX: "); break;
-                    case 4: s3pi.printf("  AccY: "); break;
-                    case 5: s3pi.printf("  AccZ: "); break;
-                    case 6: s3pi.printf("  MagX: "); break;
-                    case 7: s3pi.printf("  MagY: "); break;
-                    case 8: s3pi.printf("  MagZ: "); break;
-                    case 9: s3pi.printf(" BG IR: "); break;
-                    case 10: s3pi.printf(" Dist1: "); break;
-                    case 11: s3pi.printf(" Dist2: "); break;
-                    case 12: s3pi.printf(" Dist3: "); break;
-                    case 13: s3pi.printf(" Dist4: "); break;
-                    case 14: s3pi.printf(" Dist5: "); break;
-                    case 15: s3pi.printf(" Dist6: "); break;
-                    case 16: s3pi.printf(" Dist7: "); break;
-                    case 17: s3pi.printf(" Dist8: "); break;
-                }
-                current_mode ++;
-                if(current_mode == 18) current_mode = 0;
-            }
-            s3pi.locate(0,1);
-            s3pi.printf("%.1f  ",sum);
-            sum = 0;
-        }
-        wait(0.05);
-    }
+void switch_pressed() {
+    //Switch(es) pressed {1 = Center  2 = Right  4 = Left  8 = Down  16 = Up}
+    char switches = piswarm.get_switches();
+  
+    //Do something...
 }