Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
Diff: psiswarm.cpp
- Revision:
- 17:bf614e28668f
- Parent:
- 16:50686c07ad07
- Child:
- 18:9204f74069b4
diff -r 50686c07ad07 -r bf614e28668f psiswarm.cpp --- 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