Edgar Buchanan
Aggregation-Flocking_2
Dependencies: Pi_Swarm_Library_v06_alpha mbed
Fork of
Pi_Swarm_Blank
by 
piswarm
Diff: main.cpp
- Revision:
- 0:46cd1498a39a
- Child:
- 1:37502eb3b70f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Nov 11 00:01:15 2013 +0000
@@ -0,0 +1,199 @@
+// York Robotics Laboratory
+// 3-Pi Swarm Robot Test Code
+// Version 0.1 - 11th November, 2013
+// James Hilder, University of York
+
+#include "mbed.h"
+#include "s3pi.h"
+
+s3pi s3pi;
+
+int line_data[5];
+char shift_mode = 0;
+char current_mode = 0;
+char cycle_mode = 0;
+
+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);
+
+ 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;
+ }
+}
+
+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);
+}
+
+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);
+ }
+}