James Hilder
Psi Swarm robot library version 0.9
Fork of
PsiSwarmV9
by 
Psi Swarm Robot
Diff: psiswarm.cpp
- Revision:
- 17:bf614e28668f
- Parent:
- 16:50686c07ad07
- Child:
- 18:9204f74069b4
--- a/psiswarm.cpp Thu Jun 01 23:02:32 2017 +0000
+++ b/psiswarm.cpp Sun Jun 04 13:11:09 2017 +0000
@@ -1,11 +1,11 @@
/* University of York Robotics Laboratory PsiSwarm Library: PsiSwarm C++ Core Source File
- *
+ *
* Copyright 2017 University of York
*
- * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
* You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
* Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and limitations under the License.
*
* File: psiswarm.cpp
@@ -15,7 +15,7 @@
*
* PsiSwarm Library Version: 0.9
*
- * May 2017
+ * June 2017
*
*
*/
@@ -55,7 +55,6 @@
DigitalOut mbed_led3(LED3);
DigitalOut mbed_led4(LED4);
-
float center_led_brightness;
float backlight_brightness;
@@ -103,16 +102,17 @@
char switch_byte;
char previous_switch_byte;
-
char use_motor_calibration = USE_MOTOR_CALIBRATION;
char motor_calibration_set;
+char base_ir_calibration_set;
+char base_colour_calibration_set;
float left_motor_calibration_value = 1.0;
float right_motor_calibration_value = 1.0;
char debug_mode = DEBUG_MODE;
char debug_output = DEBUG_OUTPUT_STREAM;
-char firmware_bytes[21];
+char firmware_bytes[80];
int base_colour_sensor_raw_values [4];
int top_colour_sensor_raw_values [4];
@@ -147,7 +147,6 @@
char user_code_restore_mode = 0;
char system_warnings = 0;
-
vector<string> basic_filenames; //filenames are stored in a vector string
char psi_basic_file_count = 0;
char use_flash_basic = 0;
@@ -156,9 +155,6 @@
int ir_pulse_delay = DEFAULT_IR_PULSE_DELAY;
int base_ir_pulse_delay = DEFAULT_BASE_IR_PULSE_DELAY;
-
-
-
/**
* init()
*
@@ -174,35 +170,63 @@
uptime.start();
primary_i2c.frequency(400000);
serial.setup_serial_interfaces();
- debug("PsiSwarm Robot Library %1.2f\n\n",SOFTWARE_VERSION_CODE);
+ debug("\n_________________________________________________________________________\n");
+ debug("--------------------PsiSwarm Robot Library %1.2f--------------------------\n",SOFTWARE_VERSION_CODE);
+ debug("_________________________________________________________________________\n");
+ debug("- Setting up serial interface\n");
debug("- Set up display\n");
display.init_display_start();
- debug("- Setting up serial interface\n");
+ debug("- Setting up GPIO expansion\n");
+ i2c_setup.IF_setup_gpio_expansion_ic();
debug("- Reading firmware: ");
if(eprom.read_firmware() == 1) {
debug("Version %3.2f\n",firmware_version);
IF_get_hardware_description();
- if(use_motor_calibration){
- if(!motor_calibration_set){
- if(firmware_version < 1.1){
- debug("- WARNING: This firmware is incompatible with motor calibration\n");
+ if(use_motor_calibration) {
+ if(!motor_calibration_set) {
+ if(firmware_version < 1.1) {
+ debug("- WARNING: Firmware incompatible with motor calibration\n");
debug("- WARNING: Please update the firmware to use this feature.\n");
- use_motor_calibration = 0;
- }
- else {
+ use_motor_calibration = 0;
+ } else {
debug("- WARNING: Motor calibration values have not been stored in firmware\n");
- debug("- WARNING: Run calibration routine to use this feature.\n");
- use_motor_calibration = 0;
+ debug("- WARNING: Run motor calibration routine to use this feature.\n");
+ use_motor_calibration = 0;
}
- }
- else {
- debug("- Motor calibration in use [LEFT:%0.4f RIGHT:%0.4f]",left_motor_calibration_value,right_motor_calibration_value);
+ } else {
+ debug("- Motor calibration in use [LEFT:%0.4f RIGHT:%0.4f]",left_motor_calibration_value,right_motor_calibration_value);
}
}
+ if(base_ir_calibration_set != 1) {
+ if(firmware_version < 1.2) {
+ debug("- WARNING: Firmware incompatible with base IR sensor calibration\n");
+ debug("- WARNING: Please update the firmware to use this feature.\n");
+ } else {
+ debug("- WARNING: Base IR calibration values not stored in firmware\n");
+ debug("- WARNING: Run sensor calibration routine to use this feature.\n");
+ }
+ // Set default calibration values for base IR sensor
+ sensors.IF_set_base_calibration_values(BIR1W,BIR2W,BIR3W,BIR4W,BIR5W,BIR1B,BIR2B,BIR3B,BIR4B,BIR5B);
+ } else debug("- Using base IR calibration values stored in firmware\n");
+ if(has_base_colour_sensor == 1 && base_colour_calibration_set != 1){
+ if(firmware_version < 1.2) {
+ debug("- WARNING: This firmware is incompatible with base colour sensor calibration\n");
+ debug("- WARNING: Please update the firmware to use this feature.\n");
+ } else {
+ debug("- WARNING: Base colour sensor calibration values not stored in firmware\n");
+ debug("- WARNING: Run sensor calibration routine to use this feature.\n");
+ }
+ // Set default calibration values for base colour sensor
+ colour.set_calibration_values(CS_C_BLACK,CS_R_BLACK,CS_G_BLACK,CS_B_BLACK,CS_C_WHITE,CS_R_WHITE,CS_G_WHITE,CS_B_WHITE);
+ } else debug("- Using base colour sensor calibration values stored in firmware\n");
+ if(firmware_version < TARGET_FIRMWARE_VERSION && AUTO_UPDATE_FIRMWARE == 1) eprom.update_firmware();
} else {
debug("INVALID\n");
- debug("- WARNING: Check firmware to enable robot features");
+ debug("- WARNING: Check firmware to enable robot features\n");
}
+
+
+
if(ENABLE_BASIC == 1) {
basic.read_list_of_file_names();
if(psi_basic_file_count == 0) {
@@ -219,19 +243,16 @@
}
debug("- Setting up motor drivers\n");
motors.init_motors();
- debug("- Setting up GPIO expansion\n");
reset_encoders();
- i2c_setup.IF_setup_gpio_expansion_ic();
if(has_temperature_sensor) {
debug("- Setting up temperature sensor\n");
i2c_setup.IF_setup_temperature_sensor();
}
if(has_base_colour_sensor) {
debug("- Setting up base colour sensor\n");
- if(colour.IF_check_base_colour_sensor() == 1){
- colour.colour_sensor_init();
- }
- else debug("- WARNING: Invalid response from colour sensor");
+ if(colour.IF_check_base_colour_sensor() == 1) {
+ colour.colour_sensor_init();
+ } else debug("- WARNING: Invalid response from colour sensor");
}
if(has_ultrasonic_sensor) {
debug("- Setting up ultrasonic sensor\n");
@@ -244,8 +265,8 @@
debug("- Battery Voltage: %1.3fV\n",sensors.get_battery_voltage());
debug("- DC Voltage : %1.3fV\n",sensors.get_dc_voltage());
debug("- Current Draw : %1.3fA\n",sensors.get_current());
- if(has_temperature_sensor){
- debug("- Temperature : %1.3fC\n",sensors.get_temperature());
+ if(has_temperature_sensor) {
+ debug("- Temperature : %1.3fC\n",sensors.get_temperature());
}
char demo_on = 0;
if(ENABLE_DEMO == 1 && switchstate > 0) demo_on=1;
@@ -258,7 +279,7 @@
if(demo_on > 0) demo.start_demo_mode();
display.init_display_end(0);
}
- debug("___________________________________________________");
+ debug("_________________________________________________________________________\n\n");
}
void Psiswarm::IF_update_minutes()
@@ -361,19 +382,19 @@
void Psiswarm::IF_get_hardware_description()
{
- debug("- Robot serial number %1.2f\n",serial_number);
- debug("- PCB version %1.2f\n",pcb_version);
- debug("- Hardware: ");
- if(has_compass) debug("COMPASS, ");
- if(has_side_ir) debug("SIDE IR, ");
- if(has_base_ir) debug("BASE IR, ");
- if(has_base_colour_sensor) debug("BASE COLOUR, ");
- if(has_top_colour_sensor) debug("TOP COLOUR, ");
- if(has_wheel_encoders) debug("WHEEL ENC., ");
- if(has_audio_pic) debug("AUDIO, ");
- if(has_ultrasonic_sensor) debug("ULTRASONIC, ");
- if(has_temperature_sensor) debug("TEMPERATURE, ");
- if(has_recharging_circuit) debug("RECHARGING, ");
- if(has_433_radio) debug("433 RADIO.");
- debug("\n");
+ debug("- Robot serial number %1.2f\n",serial_number);
+ debug("- PCB version %1.2f\n",pcb_version);
+ debug("- Hardware: ");
+ if(has_compass) debug("COMPASS ");
+ if(has_side_ir) debug("SIDE-IR ");
+ if(has_base_ir) debug("BASE-IR ");
+ if(has_base_colour_sensor) debug("BASE-COLOUR ");
+ if(has_top_colour_sensor) debug("TOP-COLOUR ");
+ if(has_wheel_encoders) debug("WHEEL-ENC ");
+ if(has_audio_pic) debug("AUDIO ");
+ if(has_ultrasonic_sensor) debug("ULTRASONIC ");
+ if(has_temperature_sensor) debug("TEMPERATURE ");
+ if(has_recharging_circuit) debug("RECHARGING ");
+ if(has_433_radio) debug("433-RADIO");
+ debug("\n");
}
\ No newline at end of file