Pi Swarm Robot
Blank program for Pi Swarm Robot
Dependencies: Pi_Swarm_Library mbed
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...
}