Edgar Buchanan / Mbed 2 deprecated Aggregation-Flocking_2

Aggregation-Flocking_2

Dependencies:   Pi_Swarm_Library_v06_alpha mbed

Fork of Pi_Swarm_Blank by piswarm

main.cpp@0:46cd1498a39a, 2013-11-11 (annotated)

Committer:
jah128
Date:
Mon Nov 11 00:01:15 2013 +0000
Revision:
0:46cd1498a39a
Child:
1:37502eb3b70f
11/11/2013 V0.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:46cd1498a39a 1 // York Robotics Laboratory
jah128 0:46cd1498a39a 2 // 3-Pi Swarm Robot Test Code
jah128 0:46cd1498a39a 3 // Version 0.1 - 11th November, 2013
jah128 0:46cd1498a39a 4 // James Hilder, University of York
jah128 0:46cd1498a39a 5
jah128 0:46cd1498a39a 6 #include "mbed.h"
jah128 0:46cd1498a39a 7 #include "s3pi.h"
jah128 0:46cd1498a39a 8
jah128 0:46cd1498a39a 9 s3pi s3pi;
jah128 0:46cd1498a39a 10
jah128 0:46cd1498a39a 11 int line_data[5];
jah128 0:46cd1498a39a 12 char shift_mode = 0;
jah128 0:46cd1498a39a 13 char current_mode = 0;
jah128 0:46cd1498a39a 14 char cycle_mode = 0;
jah128 0:46cd1498a39a 15
jah128 0:46cd1498a39a 16 void switch_pressed() {
jah128 0:46cd1498a39a 17 //Switch pressed
jah128 0:46cd1498a39a 18 //1 = Center
jah128 0:46cd1498a39a 19 //2 = Right
jah128 0:46cd1498a39a 20 //4 = Left
jah128 0:46cd1498a39a 21 //8 = Down
jah128 0:46cd1498a39a 22 //16 = Up
jah128 0:46cd1498a39a 23 char switches = s3pi.get_switches();
jah128 0:46cd1498a39a 24 //s3pi.locate(0,1);
jah128 0:46cd1498a39a 25 //s3pi.printf("%d ", switches);
jah128 0:46cd1498a39a 26
jah128 0:46cd1498a39a 27 if(switches == 1) {
jah128 0:46cd1498a39a 28 shift_mode = 1;
jah128 0:46cd1498a39a 29 cycle_mode = 1-cycle_mode;
jah128 0:46cd1498a39a 30 }
jah128 0:46cd1498a39a 31
jah128 0:46cd1498a39a 32 if(switches == 4) {
jah128 0:46cd1498a39a 33 shift_mode = 1;
jah128 0:46cd1498a39a 34 if(current_mode == 0) current_mode = 17;
jah128 0:46cd1498a39a 35 else current_mode --;
jah128 0:46cd1498a39a 36 }
jah128 0:46cd1498a39a 37 if(switches == 2) {
jah128 0:46cd1498a39a 38 shift_mode = 1;
jah128 0:46cd1498a39a 39 current_mode ++;
jah128 0:46cd1498a39a 40 if (current_mode > 17) current_mode =0;
jah128 0:46cd1498a39a 41 }
jah128 0:46cd1498a39a 42 }
jah128 0:46cd1498a39a 43
jah128 0:46cd1498a39a 44 void init() {
jah128 0:46cd1498a39a 45 //Initialisation routine for 3-Pi Robot
jah128 0:46cd1498a39a 46 //Displays a message on the screen and flashes the central LED
jah128 0:46cd1498a39a 47 //Calibrates the gyro and accelerometer
jah128 0:46cd1498a39a 48 //Make sure robot is flat and stationary when this code is run (calibration starts after about 1 second to allow robot to settle)
jah128 0:46cd1498a39a 49 s3pi.play_tune("MSCEG>C",7);
jah128 0:46cd1498a39a 50 s3pi.cls();
jah128 0:46cd1498a39a 51 s3pi.enable_cled(1);
jah128 0:46cd1498a39a 52 s3pi.set_cled_colour(200,0,0);
jah128 0:46cd1498a39a 53 s3pi.printf(" YORK ");
jah128 0:46cd1498a39a 54 s3pi.locate(0,1);
jah128 0:46cd1498a39a 55 s3pi.printf("ROBOTLAB");
jah128 0:46cd1498a39a 56 wait(0.3);
jah128 0:46cd1498a39a 57 s3pi.set_cled_colour(0,200,0);
jah128 0:46cd1498a39a 58 wait(0.3);
jah128 0:46cd1498a39a 59 s3pi.set_cled_colour(0,0,200);
jah128 0:46cd1498a39a 60 wait(0.3);
jah128 0:46cd1498a39a 61 s3pi.cls();
jah128 0:46cd1498a39a 62 s3pi.set_cled_colour(20,20,20);
jah128 0:46cd1498a39a 63 s3pi.printf("3piSwarm");
jah128 0:46cd1498a39a 64 s3pi.locate(0,1);
jah128 0:46cd1498a39a 65 s3pi.printf("ID : %d ",s3pi.get_id());
jah128 0:46cd1498a39a 66 if(s3pi.calibrate_magnetometer() != 0){
jah128 0:46cd1498a39a 67 s3pi.cls();
jah128 0:46cd1498a39a 68 s3pi.locate(0,0);
jah128 0:46cd1498a39a 69 s3pi.printf("Mag Cal ");
jah128 0:46cd1498a39a 70 s3pi.locate(0,1);
jah128 0:46cd1498a39a 71 s3pi.printf("Failed ");
jah128 0:46cd1498a39a 72 wait(0.5);
jah128 0:46cd1498a39a 73 }else{
jah128 0:46cd1498a39a 74 if(s3pi.calibrate_gyro() == 0){
jah128 0:46cd1498a39a 75 s3pi.cls();
jah128 0:46cd1498a39a 76 s3pi.locate(0,0);
jah128 0:46cd1498a39a 77 s3pi.printf("Gyro Cal");
jah128 0:46cd1498a39a 78 s3pi.locate(0,1);
jah128 0:46cd1498a39a 79 s3pi.printf("Failed ");
jah128 0:46cd1498a39a 80 wait(0.5);
jah128 0:46cd1498a39a 81 }else{
jah128 0:46cd1498a39a 82 if(s3pi.calibrate_accelerometer() == 0){
jah128 0:46cd1498a39a 83 s3pi.cls();
jah128 0:46cd1498a39a 84 s3pi.locate(0,0);
jah128 0:46cd1498a39a 85 s3pi.printf("Acc. Cal");
jah128 0:46cd1498a39a 86 s3pi.locate(0,1);
jah128 0:46cd1498a39a 87 s3pi.printf("Failed ");
jah128 0:46cd1498a39a 88 wait(0.5);
jah128 0:46cd1498a39a 89 }
jah128 0:46cd1498a39a 90 }
jah128 0:46cd1498a39a 91 }
jah128 0:46cd1498a39a 92 wait(1.5);
jah128 0:46cd1498a39a 93 s3pi.cls();
jah128 0:46cd1498a39a 94 s3pi.set_cled_colour(10,20,30);
jah128 0:46cd1498a39a 95 wait(1.5);
jah128 0:46cd1498a39a 96 }
jah128 0:46cd1498a39a 97
jah128 0:46cd1498a39a 98 int main() {
jah128 0:46cd1498a39a 99 init();
jah128 0:46cd1498a39a 100 s3pi.locate(0,0);
jah128 0:46cd1498a39a 101 s3pi.printf(" Gyro:");
jah128 0:46cd1498a39a 102 int count = 0;
jah128 0:46cd1498a39a 103 float sum = 0;
jah128 0:46cd1498a39a 104 while(1) {
jah128 0:46cd1498a39a 105 count++;
jah128 0:46cd1498a39a 106 if(shift_mode == 1) {
jah128 0:46cd1498a39a 107 shift_mode = 0;
jah128 0:46cd1498a39a 108 count = 0;
jah128 0:46cd1498a39a 109 sum = 0;
jah128 0:46cd1498a39a 110 s3pi.cls();
jah128 0:46cd1498a39a 111 s3pi.locate(0,0);
jah128 0:46cd1498a39a 112 if(cycle_mode == 1) {
jah128 0:46cd1498a39a 113 s3pi.printf("CYCLE ON");
jah128 0:46cd1498a39a 114 }
jah128 0:46cd1498a39a 115 else{
jah128 0:46cd1498a39a 116 switch(current_mode) {
jah128 0:46cd1498a39a 117 case 0: s3pi.printf(" Gyro: "); break;
jah128 0:46cd1498a39a 118 case 1: s3pi.printf(" Temp: "); break;
jah128 0:46cd1498a39a 119 case 2: s3pi.printf(" Light: "); break;
jah128 0:46cd1498a39a 120 case 3: s3pi.printf(" AccX: "); break;
jah128 0:46cd1498a39a 121 case 4: s3pi.printf(" AccY: "); break;
jah128 0:46cd1498a39a 122 case 5: s3pi.printf(" AccZ: "); break;
jah128 0:46cd1498a39a 123 case 6: s3pi.printf(" MagX: "); break;
jah128 0:46cd1498a39a 124 case 7: s3pi.printf(" MagY: "); break;
jah128 0:46cd1498a39a 125 case 8: s3pi.printf(" MagZ: "); break;
jah128 0:46cd1498a39a 126 case 9: s3pi.printf(" BG IR: "); break;
jah128 0:46cd1498a39a 127 case 10: s3pi.printf(" Dist1: "); break;
jah128 0:46cd1498a39a 128 case 11: s3pi.printf(" Dist2: "); break;
jah128 0:46cd1498a39a 129 case 12: s3pi.printf(" Dist3: "); break;
jah128 0:46cd1498a39a 130 case 13: s3pi.printf(" Dist4: "); break;
jah128 0:46cd1498a39a 131 case 14: s3pi.printf(" Dist5: "); break;
jah128 0:46cd1498a39a 132 case 15: s3pi.printf(" Dist6: "); break;
jah128 0:46cd1498a39a 133 case 16: s3pi.printf(" Dist7: "); break;
jah128 0:46cd1498a39a 134 case 17: s3pi.printf(" Dist8: "); break;
jah128 0:46cd1498a39a 135 }
jah128 0:46cd1498a39a 136 }
jah128 0:46cd1498a39a 137 }
jah128 0:46cd1498a39a 138 switch(current_mode) {
jah128 0:46cd1498a39a 139 case 0: sum+=s3pi.read_gyro(); break;
jah128 0:46cd1498a39a 140 case 1: sum+=s3pi.read_temperature(); break;
jah128 0:46cd1498a39a 141 case 2: sum+=s3pi.read_light_sensor(); break;
jah128 0:46cd1498a39a 142 case 3: sum+=s3pi.read_accelerometer_x(); break;
jah128 0:46cd1498a39a 143 case 4: sum+=s3pi.read_accelerometer_y(); break;
jah128 0:46cd1498a39a 144 case 5: sum+=s3pi.read_accelerometer_z(); break;
jah128 0:46cd1498a39a 145 case 6: s3pi.read_magnetometer(); sum+=s3pi.get_magnetometer_x(); break;
jah128 0:46cd1498a39a 146 case 7: s3pi.read_magnetometer(); sum+=s3pi.get_magnetometer_y(); break;
jah128 0:46cd1498a39a 147 case 8: s3pi.read_magnetometer(); sum+=s3pi.get_magnetometer_z(); break;
jah128 0:46cd1498a39a 148 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;
jah128 0:46cd1498a39a 149 case 10: sum+=s3pi.read_reflected_ir_distance(0); break;
jah128 0:46cd1498a39a 150 case 11: sum+=s3pi.read_reflected_ir_distance(1); break;
jah128 0:46cd1498a39a 151 case 12: sum+=s3pi.read_reflected_ir_distance(2); break;
jah128 0:46cd1498a39a 152 case 13: sum+=s3pi.read_reflected_ir_distance(3); break;
jah128 0:46cd1498a39a 153 case 14: sum+=s3pi.read_reflected_ir_distance(4); break;
jah128 0:46cd1498a39a 154 case 15: sum+=s3pi.read_reflected_ir_distance(5); break;
jah128 0:46cd1498a39a 155 case 16: sum+=s3pi.read_reflected_ir_distance(6); break;
jah128 0:46cd1498a39a 156 case 17: sum+=s3pi.read_reflected_ir_distance(7); break;
jah128 0:46cd1498a39a 157 }
jah128 0:46cd1498a39a 158 s3pi.read_raw_sensors(line_data);
jah128 0:46cd1498a39a 159 int red = (line_data [0] - 50) / 38;
jah128 0:46cd1498a39a 160 int green = (line_data [1] - 50) / 38;
jah128 0:46cd1498a39a 161 int blue = (line_data [2] - 50) / 38;
jah128 0:46cd1498a39a 162 s3pi.set_cled_colour(red,green,blue);
jah128 0:46cd1498a39a 163 s3pi.set_oled_colour(red,green,blue);
jah128 0:46cd1498a39a 164 if (count>9){
jah128 0:46cd1498a39a 165 sum /= count;
jah128 0:46cd1498a39a 166 count = 0;
jah128 0:46cd1498a39a 167 if(cycle_mode == 1){
jah128 0:46cd1498a39a 168 s3pi.cls();
jah128 0:46cd1498a39a 169 s3pi.locate(0,0);
jah128 0:46cd1498a39a 170 switch(current_mode) {
jah128 0:46cd1498a39a 171 case 0: s3pi.printf(" Gyro: "); break;
jah128 0:46cd1498a39a 172 case 1: s3pi.printf(" Temp: "); break;
jah128 0:46cd1498a39a 173 case 2: s3pi.printf(" Light: "); break;
jah128 0:46cd1498a39a 174 case 3: s3pi.printf(" AccX: "); break;
jah128 0:46cd1498a39a 175 case 4: s3pi.printf(" AccY: "); break;
jah128 0:46cd1498a39a 176 case 5: s3pi.printf(" AccZ: "); break;
jah128 0:46cd1498a39a 177 case 6: s3pi.printf(" MagX: "); break;
jah128 0:46cd1498a39a 178 case 7: s3pi.printf(" MagY: "); break;
jah128 0:46cd1498a39a 179 case 8: s3pi.printf(" MagZ: "); break;
jah128 0:46cd1498a39a 180 case 9: s3pi.printf(" BG IR: "); break;
jah128 0:46cd1498a39a 181 case 10: s3pi.printf(" Dist1: "); break;
jah128 0:46cd1498a39a 182 case 11: s3pi.printf(" Dist2: "); break;
jah128 0:46cd1498a39a 183 case 12: s3pi.printf(" Dist3: "); break;
jah128 0:46cd1498a39a 184 case 13: s3pi.printf(" Dist4: "); break;
jah128 0:46cd1498a39a 185 case 14: s3pi.printf(" Dist5: "); break;
jah128 0:46cd1498a39a 186 case 15: s3pi.printf(" Dist6: "); break;
jah128 0:46cd1498a39a 187 case 16: s3pi.printf(" Dist7: "); break;
jah128 0:46cd1498a39a 188 case 17: s3pi.printf(" Dist8: "); break;
jah128 0:46cd1498a39a 189 }
jah128 0:46cd1498a39a 190 current_mode ++;
jah128 0:46cd1498a39a 191 if(current_mode == 18) current_mode = 0;
jah128 0:46cd1498a39a 192 }
jah128 0:46cd1498a39a 193 s3pi.locate(0,1);
jah128 0:46cd1498a39a 194 s3pi.printf("%.1f ",sum);
jah128 0:46cd1498a39a 195 sum = 0;
jah128 0:46cd1498a39a 196 }
jah128 0:46cd1498a39a 197 wait(0.05);
jah128 0:46cd1498a39a 198 }
jah128 0:46cd1498a39a 199 }