Homero Silva
/
PRGP_Pi_Swarm_ground_search_algorithm
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
Pi_Swarm_Blank
by 
James Hilder
main.cpp
- Committer:
- jah128
- Date:
- 2013-11-11
- Revision:
- 0:46cd1498a39a
- Child:
- 1:37502eb3b70f
File content as of revision 0:46cd1498a39a:
// 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);
}
}