Psi Swarm Robot / PsiSwarmV9

Psi Swarm robot library version 0.9

Dependents:   PsiSwarm_V9_Blank

Fork of PsiSwarmV9 by James Hilder

psiswarm.cpp@20:2b6ebe60929d, 2019-07-08 (annotated)

Committer:
richardredpath
Date:
Mon Jul 08 10:50:40 2019 +0000
Revision:
20:2b6ebe60929d
Parent:
19:3e3b03d80ea3 
Fixed deprecated warnings for callbacks throughout the library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 12:878c6e9d9e60 1 /* University of York Robotics Laboratory PsiSwarm Library: PsiSwarm C++ Core Source File
jah128 17:bf614e28668f 2 *
jah128 15:66be5ec52c3b 3 * Copyright 2017 University of York
jah128 6:b340a527add9 4 *
jah128 17:bf614e28668f 5 * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
jah128 6:b340a527add9 6 * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
jah128 6:b340a527add9 7 * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
jah128 17:bf614e28668f 8 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
jah128 6:b340a527add9 9 * See the License for the specific language governing permissions and limitations under the License.
jah128 1:060690a934a9 10 *
jah128 0:d6269d17c8cf 11 * File: psiswarm.cpp
jah128 0:d6269d17c8cf 12 *
jah128 0:d6269d17c8cf 13 * (C) Dept. Electronics & Computer Science, University of York
jah128 0:d6269d17c8cf 14 * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
jah128 0:d6269d17c8cf 15 *
jah128 15:66be5ec52c3b 16 * PsiSwarm Library Version: 0.9
jah128 0:d6269d17c8cf 17 *
jah128 17:bf614e28668f 18 * June 2017
jah128 0:d6269d17c8cf 19 *
jah128 0:d6269d17c8cf 20 *
jah128 1:060690a934a9 21 */
jah128 0:d6269d17c8cf 22
jah128 0:d6269d17c8cf 23 #include "psiswarm.h"
jah128 0:d6269d17c8cf 24
jah128 8:6c92789d5f87 25 //Setup class instances
jah128 8:6c92789d5f87 26 Display display; //Connects to i2c(p28,p27), reset(p29), backlight(p30)
jah128 8:6c92789d5f87 27 Motors motors;
jah128 8:6c92789d5f87 28 Eprom eprom;
jah128 9:dde9e21030eb 29 Led led;
jah128 10:e58323951c08 30 Sensors sensors;
jah128 11:312663037b8c 31 SerialControl serial;
jah128 11:312663037b8c 32 Sound sound;
jah128 11:312663037b8c 33 Setup i2c_setup;
jah128 11:312663037b8c 34 Demo demo;
jah128 12:878c6e9d9e60 35 Animations animations;
jah128 12:878c6e9d9e60 36 Basic basic;
jah128 13:adbd827234a4 37 Colour colour;
jah128 8:6c92789d5f87 38
jah128 0:d6269d17c8cf 39 //Setup MBED connections to PsiSwarm Robot
jah128 0:d6269d17c8cf 40 Serial pc(USBTX,USBRX);
jah128 0:d6269d17c8cf 41 I2C primary_i2c (p9, p10);
jah128 0:d6269d17c8cf 42 InterruptIn gpio_interrupt (p12);
jah128 0:d6269d17c8cf 43 Serial bt(p13, p14);
jah128 0:d6269d17c8cf 44 AnalogIn vin_current(p15);
jah128 0:d6269d17c8cf 45 AnalogIn vin_battery(p16);
jah128 0:d6269d17c8cf 46 AnalogIn vin_dc(p17);
jah128 0:d6269d17c8cf 47 PwmOut motor_left_f (p21);
jah128 0:d6269d17c8cf 48 PwmOut motor_left_r (p22);
jah128 0:d6269d17c8cf 49 PwmOut motor_right_f(p23);
jah128 0:d6269d17c8cf 50 PwmOut motor_right_r(p24);
jah128 0:d6269d17c8cf 51 PwmOut center_led_red(p25);
jah128 0:d6269d17c8cf 52 PwmOut center_led_green(p26);
jah128 0:d6269d17c8cf 53 DigitalOut mbed_led1(LED1);
jah128 0:d6269d17c8cf 54 DigitalOut mbed_led2(LED2);
jah128 0:d6269d17c8cf 55 DigitalOut mbed_led3(LED3);
jah128 0:d6269d17c8cf 56 DigitalOut mbed_led4(LED4);
jah128 0:d6269d17c8cf 57
jah128 0:d6269d17c8cf 58 float center_led_brightness;
jah128 0:d6269d17c8cf 59 float backlight_brightness;
jah128 0:d6269d17c8cf 60
jah128 0:d6269d17c8cf 61 Ticker event_handler;
jah128 0:d6269d17c8cf 62 Timer uptime;
jah128 0:d6269d17c8cf 63 Timeout pause_usercode_timeout;
jah128 0:d6269d17c8cf 64 Ticker ultrasonic_ticker;
jah128 0:d6269d17c8cf 65 Timeout ultrasonic_timeout;
jah128 0:d6269d17c8cf 66 int timer_minute_count;
jah128 0:d6269d17c8cf 67 Ticker timer_ticker;
jah128 0:d6269d17c8cf 68
jah128 0:d6269d17c8cf 69 float firmware_version;
jah128 1:060690a934a9 70 float pcb_version;
jah128 1:060690a934a9 71 float serial_number;
jah128 1:060690a934a9 72
jah128 1:060690a934a9 73 char has_compass=0;
jah128 1:060690a934a9 74 char has_side_ir=1;
jah128 1:060690a934a9 75 char has_base_ir=1;
jah128 1:060690a934a9 76 char has_base_colour_sensor=0;
jah128 1:060690a934a9 77 char has_top_colour_sensor=0;
jah128 1:060690a934a9 78 char has_wheel_encoders=0;
jah128 1:060690a934a9 79 char has_audio_pic=0;
jah128 1:060690a934a9 80 char has_ultrasonic_sensor=0;
jah128 1:060690a934a9 81 char has_temperature_sensor=0;
jah128 1:060690a934a9 82 char has_recharging_circuit=0;
jah128 1:060690a934a9 83 char has_433_radio=0;
jah128 1:060690a934a9 84
jah128 0:d6269d17c8cf 85 char robot_id;
jah128 0:d6269d17c8cf 86 char previous_robot_id;
jah128 0:d6269d17c8cf 87
jah128 0:d6269d17c8cf 88 char wheel_encoder_byte;
jah128 0:d6269d17c8cf 89 char previous_wheel_encoder_byte;
jah128 0:d6269d17c8cf 90 signed int left_encoder;
jah128 0:d6269d17c8cf 91 signed int right_encoder;
jah128 0:d6269d17c8cf 92
jah128 0:d6269d17c8cf 93 char time_based_motor_action = 0;
jah128 0:d6269d17c8cf 94
jah128 0:d6269d17c8cf 95 char testing_voltage_regulators_flag = 1;
jah128 0:d6269d17c8cf 96 char power_good_motor_left = 2;
jah128 0:d6269d17c8cf 97 char power_good_motor_right = 2;
jah128 0:d6269d17c8cf 98 char power_good_infrared = 2;
jah128 0:d6269d17c8cf 99 char status_dc_in = 2;
jah128 0:d6269d17c8cf 100 char status_charging = 2;
jah128 0:d6269d17c8cf 101
jah128 0:d6269d17c8cf 102 char switch_byte;
jah128 0:d6269d17c8cf 103 char previous_switch_byte;
jah128 0:d6269d17c8cf 104
jah128 4:1c621cb8cf0d 105 char use_motor_calibration = USE_MOTOR_CALIBRATION;
jah128 4:1c621cb8cf0d 106 char motor_calibration_set;
jah128 17:bf614e28668f 107 char base_ir_calibration_set;
jah128 17:bf614e28668f 108 char base_colour_calibration_set;
jah128 4:1c621cb8cf0d 109 float left_motor_calibration_value = 1.0;
jah128 4:1c621cb8cf0d 110 float right_motor_calibration_value = 1.0;
jah128 18:9204f74069b4 111 float left_motor_stall_offset = 0.0;
jah128 18:9204f74069b4 112 float right_motor_stall_offset = 0.0;
jah128 4:1c621cb8cf0d 113
jah128 0:d6269d17c8cf 114 char debug_mode = DEBUG_MODE;
jah128 0:d6269d17c8cf 115 char debug_output = DEBUG_OUTPUT_STREAM;
jah128 1:060690a934a9 116
jah128 17:bf614e28668f 117 char firmware_bytes[80];
jah128 0:d6269d17c8cf 118
jah128 0:d6269d17c8cf 119 int base_colour_sensor_raw_values [4];
jah128 0:d6269d17c8cf 120 int top_colour_sensor_raw_values [4];
jah128 0:d6269d17c8cf 121
jah128 0:d6269d17c8cf 122 char waiting_for_ultrasonic = 0;
jah128 0:d6269d17c8cf 123 int ultrasonic_distance = 0;
jah128 0:d6269d17c8cf 124 char ultrasonic_distance_updated = 0;
jah128 0:d6269d17c8cf 125
jah128 0:d6269d17c8cf 126
jah128 0:d6269d17c8cf 127 float line_position = 0;
jah128 0:d6269d17c8cf 128 char line_found = 0;
jah128 0:d6269d17c8cf 129
jah128 0:d6269d17c8cf 130 unsigned short background_ir_values [8];
jah128 0:d6269d17c8cf 131 unsigned short illuminated_ir_values [8];
jah128 0:d6269d17c8cf 132 float reflected_ir_distances [8];
jah128 0:d6269d17c8cf 133 char ir_values_stored = 0;
jah128 0:d6269d17c8cf 134 unsigned short background_base_ir_values [5];
jah128 0:d6269d17c8cf 135 unsigned short illuminated_base_ir_values [5];
jah128 0:d6269d17c8cf 136 char base_ir_values_stored = 0;
jah128 0:d6269d17c8cf 137
jah128 0:d6269d17c8cf 138 float motor_left_speed;
jah128 0:d6269d17c8cf 139 float motor_right_speed;
jah128 0:d6269d17c8cf 140 char motor_left_brake;
jah128 0:d6269d17c8cf 141 char motor_right_brake;
jah128 0:d6269d17c8cf 142
jah128 0:d6269d17c8cf 143 char demo_on = 0;
jah128 0:d6269d17c8cf 144 char event = 0;
jah128 0:d6269d17c8cf 145 char change_id_event = 0;
jah128 0:d6269d17c8cf 146 char encoder_event = 0;
jah128 0:d6269d17c8cf 147 char switch_event = 0;
jah128 0:d6269d17c8cf 148 char user_code_running = 0;
jah128 0:d6269d17c8cf 149 char user_code_restore_mode = 0;
jah128 0:d6269d17c8cf 150 char system_warnings = 0;
jah128 18:9204f74069b4 151 short boot_count = 0;
jah128 0:d6269d17c8cf 152
jah128 0:d6269d17c8cf 153 vector<string> basic_filenames; //filenames are stored in a vector string
jah128 0:d6269d17c8cf 154 char psi_basic_file_count = 0;
jah128 0:d6269d17c8cf 155 char use_flash_basic = 0;
jah128 0:d6269d17c8cf 156 char file_transfer_mode = 0;
jah128 0:d6269d17c8cf 157
jah128 0:d6269d17c8cf 158 int ir_pulse_delay = DEFAULT_IR_PULSE_DELAY;
jah128 0:d6269d17c8cf 159 int base_ir_pulse_delay = DEFAULT_BASE_IR_PULSE_DELAY;
jah128 0:d6269d17c8cf 160
jah128 0:d6269d17c8cf 161 /**
jah128 0:d6269d17c8cf 162 * init()
jah128 0:d6269d17c8cf 163 *
jah128 0:d6269d17c8cf 164 * Main initialisation routine for the PsiSwarm robot
jah128 0:d6269d17c8cf 165 *
jah128 0:d6269d17c8cf 166 * Set up the GPIO expansion ICs, launch demo mode if button is held
jah128 0:d6269d17c8cf 167 */
jah128 12:878c6e9d9e60 168 void Psiswarm::init()
jah128 0:d6269d17c8cf 169 {
jah128 0:d6269d17c8cf 170 firmware_version=0;
jah128 0:d6269d17c8cf 171 timer_minute_count = 0;
richardredpath 20:2b6ebe60929d 172 timer_ticker.attach(callback(this, &Psiswarm::IF_update_minutes), 300);
jah128 0:d6269d17c8cf 173 uptime.start();
jah128 0:d6269d17c8cf 174 primary_i2c.frequency(400000);
jah128 11:312663037b8c 175 serial.setup_serial_interfaces();
jah128 17:bf614e28668f 176 debug("\n_________________________________________________________________________\n");
jah128 17:bf614e28668f 177 debug("--------------------PsiSwarm Robot Library %1.2f--------------------------\n",SOFTWARE_VERSION_CODE);
jah128 17:bf614e28668f 178 debug("_________________________________________________________________________\n");
jah128 17:bf614e28668f 179 debug("- Setting up serial interface\n");
jah128 15:66be5ec52c3b 180 debug("- Set up display\n");
jah128 15:66be5ec52c3b 181 display.init_display_start();
jah128 17:bf614e28668f 182 debug("- Setting up GPIO expansion\n");
jah128 17:bf614e28668f 183 i2c_setup.IF_setup_gpio_expansion_ic();
jah128 0:d6269d17c8cf 184 debug("- Reading firmware: ");
jah128 8:6c92789d5f87 185 if(eprom.read_firmware() == 1) {
jah128 1:060690a934a9 186 debug("Version %3.2f\n",firmware_version);
jah128 1:060690a934a9 187 IF_get_hardware_description();
jah128 17:bf614e28668f 188 if(use_motor_calibration) {
jah128 17:bf614e28668f 189 if(!motor_calibration_set) {
jah128 17:bf614e28668f 190 if(firmware_version < 1.1) {
jah128 17:bf614e28668f 191 debug("- WARNING: Firmware incompatible with motor calibration\n");
jah128 15:66be5ec52c3b 192 debug("- WARNING: Please update the firmware to use this feature.\n");
jah128 17:bf614e28668f 193 use_motor_calibration = 0;
jah128 17:bf614e28668f 194 } else {
jah128 15:66be5ec52c3b 195 debug("- WARNING: Motor calibration values have not been stored in firmware\n");
jah128 17:bf614e28668f 196 debug("- WARNING: Run motor calibration routine to use this feature.\n");
jah128 17:bf614e28668f 197 use_motor_calibration = 0;
jah128 4:1c621cb8cf0d 198 }
jah128 17:bf614e28668f 199 } else {
jah128 18:9204f74069b4 200 debug("- Motor calibration in use\n- [LEFT:%0.4f (%1.2f OFFSET) RIGHT:%0.4f (%2.3f OFFSET)]\n",left_motor_calibration_value,left_motor_stall_offset,right_motor_calibration_value,right_motor_stall_offset);
jah128 4:1c621cb8cf0d 201 }
jah128 4:1c621cb8cf0d 202 }
jah128 17:bf614e28668f 203 if(base_ir_calibration_set != 1) {
jah128 17:bf614e28668f 204 if(firmware_version < 1.2) {
jah128 17:bf614e28668f 205 debug("- WARNING: Firmware incompatible with base IR sensor calibration\n");
jah128 17:bf614e28668f 206 debug("- WARNING: Please update the firmware to use this feature.\n");
jah128 17:bf614e28668f 207 } else {
jah128 17:bf614e28668f 208 debug("- WARNING: Base IR calibration values not stored in firmware\n");
jah128 17:bf614e28668f 209 debug("- WARNING: Run sensor calibration routine to use this feature.\n");
jah128 17:bf614e28668f 210 }
jah128 17:bf614e28668f 211 // Set default calibration values for base IR sensor
jah128 17:bf614e28668f 212 sensors.IF_set_base_calibration_values(BIR1W,BIR2W,BIR3W,BIR4W,BIR5W,BIR1B,BIR2B,BIR3B,BIR4B,BIR5B);
jah128 18:9204f74069b4 213 } else {
jah128 18:9204f74069b4 214 debug("- Using base IR calibration values stored in firmware\n- %s\n",sensors.IF_get_base_calibration_values_string());
jah128 18:9204f74069b4 215 }
jah128 17:bf614e28668f 216 if(has_base_colour_sensor == 1 && base_colour_calibration_set != 1){
jah128 17:bf614e28668f 217 if(firmware_version < 1.2) {
jah128 17:bf614e28668f 218 debug("- WARNING: This firmware is incompatible with base colour sensor calibration\n");
jah128 17:bf614e28668f 219 debug("- WARNING: Please update the firmware to use this feature.\n");
jah128 17:bf614e28668f 220 } else {
jah128 17:bf614e28668f 221 debug("- WARNING: Base colour sensor calibration values not stored in firmware\n");
jah128 17:bf614e28668f 222 debug("- WARNING: Run sensor calibration routine to use this feature.\n");
jah128 17:bf614e28668f 223 }
jah128 17:bf614e28668f 224 // Set default calibration values for base colour sensor
jah128 18:9204f74069b4 225
jah128 17:bf614e28668f 226 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);
jah128 18:9204f74069b4 227 } else {
jah128 18:9204f74069b4 228 debug("- Using base colour sensor calibration values stored in firmware\n- %s\n",colour.IF_get_calibration_values_string());
jah128 18:9204f74069b4 229 }
jah128 17:bf614e28668f 230 if(firmware_version < TARGET_FIRMWARE_VERSION && AUTO_UPDATE_FIRMWARE == 1) eprom.update_firmware();
jah128 18:9204f74069b4 231 if(firmware_version > 1.1) debug("- Boot Count: %d\n",boot_count);
jah128 1:060690a934a9 232 } else {
jah128 1:060690a934a9 233 debug("INVALID\n");
jah128 19:3e3b03d80ea3 234 if(ENABLE_FIRMWARE_WRITER){
jah128 19:3e3b03d80ea3 235 debug("- Starting firmware writer\n");
jah128 19:3e3b03d80ea3 236 eprom.firmware_writer();
jah128 19:3e3b03d80ea3 237 }else{
jah128 19:3e3b03d80ea3 238 debug("- WARNING: Enable firmware writer in settings or use firmware writer program to set firmware\n");
jah128 19:3e3b03d80ea3 239 }
jah128 1:060690a934a9 240 }
jah128 17:bf614e28668f 241
jah128 17:bf614e28668f 242
jah128 17:bf614e28668f 243
jah128 1:060690a934a9 244 if(ENABLE_BASIC == 1) {
jah128 12:878c6e9d9e60 245 basic.read_list_of_file_names();
jah128 1:060690a934a9 246 if(psi_basic_file_count == 0) {
jah128 1:060690a934a9 247 debug("- No PsiBasic files found\n");
jah128 1:060690a934a9 248 } else use_flash_basic = 1;
jah128 0:d6269d17c8cf 249 }
jah128 0:d6269d17c8cf 250 debug("- Setting up PIC microcontroller\n");
jah128 0:d6269d17c8cf 251 // IF_check_pic_firmware();
jah128 0:d6269d17c8cf 252 debug("- Setting up LED drivers\n");
jah128 9:dde9e21030eb 253 led.IF_init_leds();
jah128 11:312663037b8c 254 if(i2c_setup.IF_setup_led_expansion_ic() != 0) {
jah128 1:060690a934a9 255 debug("- WARNING: No I2C acknowledge for LED driver\n");
jah128 1:060690a934a9 256 system_warnings += 1;
jah128 0:d6269d17c8cf 257 }
jah128 0:d6269d17c8cf 258 debug("- Setting up motor drivers\n");
jah128 8:6c92789d5f87 259 motors.init_motors();
jah128 0:d6269d17c8cf 260 reset_encoders();
jah128 1:060690a934a9 261 if(has_temperature_sensor) {
jah128 1:060690a934a9 262 debug("- Setting up temperature sensor\n");
jah128 11:312663037b8c 263 i2c_setup.IF_setup_temperature_sensor();
jah128 1:060690a934a9 264 }
jah128 1:060690a934a9 265 if(has_base_colour_sensor) {
jah128 1:060690a934a9 266 debug("- Setting up base colour sensor\n");
jah128 17:bf614e28668f 267 if(colour.IF_check_base_colour_sensor() == 1) {
jah128 17:bf614e28668f 268 colour.colour_sensor_init();
jah128 17:bf614e28668f 269 } else debug("- WARNING: Invalid response from colour sensor");
jah128 1:060690a934a9 270 }
jah128 1:060690a934a9 271 if(has_ultrasonic_sensor) {
jah128 1:060690a934a9 272 debug("- Setting up ultrasonic sensor\n");
jah128 1:060690a934a9 273 //enable_ultrasonic_ticker();
jah128 1:060690a934a9 274 }
jah128 1:060690a934a9 275
jah128 0:d6269d17c8cf 276 debug("- Robot ID: %d\n",robot_id);
jah128 11:312663037b8c 277 char switchstate = i2c_setup.IF_get_switch_state();
jah128 0:d6269d17c8cf 278 debug("- Switch State : %d\n",switchstate);
jah128 10:e58323951c08 279 debug("- Battery Voltage: %1.3fV\n",sensors.get_battery_voltage());
jah128 10:e58323951c08 280 debug("- DC Voltage : %1.3fV\n",sensors.get_dc_voltage());
jah128 10:e58323951c08 281 debug("- Current Draw : %1.3fA\n",sensors.get_current());
jah128 17:bf614e28668f 282 if(has_temperature_sensor) {
jah128 17:bf614e28668f 283 debug("- Temperature : %1.3fC\n",sensors.get_temperature());
jah128 1:060690a934a9 284 }
jah128 11:312663037b8c 285 char demo_on = 0;
jah128 11:312663037b8c 286 if(ENABLE_DEMO == 1 && switchstate > 0) demo_on=1;
jah128 15:66be5ec52c3b 287 display.init_display_end(demo_on);
richardredpath 20:2b6ebe60929d 288 event_handler.attach_us(callback(this, &Psiswarm::IF_handle_events), 1000);
jah128 11:312663037b8c 289 if(demo_on > 0) {
jah128 0:d6269d17c8cf 290 debug("- Demo mode button is pressed\n");
jah128 15:66be5ec52c3b 291 wait(0.6);
jah128 11:312663037b8c 292 demo_on = i2c_setup.IF_get_switch_state();
jah128 11:312663037b8c 293 if(demo_on > 0) demo.start_demo_mode();
jah128 15:66be5ec52c3b 294 display.init_display_end(0);
jah128 0:d6269d17c8cf 295 }
jah128 17:bf614e28668f 296 debug("_________________________________________________________________________\n\n");
jah128 0:d6269d17c8cf 297 }
jah128 0:d6269d17c8cf 298
jah128 12:878c6e9d9e60 299 void Psiswarm::IF_update_minutes()
jah128 1:060690a934a9 300 {
jah128 0:d6269d17c8cf 301 uptime.reset();
jah128 0:d6269d17c8cf 302 timer_minute_count += 5;
jah128 0:d6269d17c8cf 303 }
jah128 0:d6269d17c8cf 304
jah128 12:878c6e9d9e60 305 void Psiswarm::IF_handle_events()
jah128 0:d6269d17c8cf 306 {
jah128 0:d6269d17c8cf 307 // This is the main 'operating system' thread that handles events from robot stimuli
jah128 0:d6269d17c8cf 308 // By default it is run every 1ms and checks if there are events to handle
jah128 1:060690a934a9 309 if(event > 0) {
jah128 0:d6269d17c8cf 310 // There are some events to handle. We don't handle all events in every loop to keep the system responsive, instead they are priorised.
jah128 1:060690a934a9 311 if(encoder_event == 1) {
jah128 0:d6269d17c8cf 312 // The encoders have changed; update the encoder values
jah128 0:d6269d17c8cf 313 IF_update_encoders();
jah128 1:060690a934a9 314 encoder_event = 0;
jah128 1:060690a934a9 315 event--;
jah128 0:d6269d17c8cf 316 } else {
jah128 1:060690a934a9 317 if(switch_event == 1) {
jah128 1:060690a934a9 318 IF_update_switch();
jah128 1:060690a934a9 319 switch_event = 0;
jah128 1:060690a934a9 320 event--;
jah128 0:d6269d17c8cf 321 }
jah128 1:060690a934a9 322 if(change_id_event == 1) {
jah128 0:d6269d17c8cf 323 // The user ID for the robot has been changed
jah128 0:d6269d17c8cf 324 IF_update_user_id();
jah128 0:d6269d17c8cf 325 change_id_event = 0;
jah128 0:d6269d17c8cf 326 event--;
jah128 0:d6269d17c8cf 327 }
jah128 0:d6269d17c8cf 328 }
jah128 1:060690a934a9 329 }
jah128 0:d6269d17c8cf 330 }
jah128 0:d6269d17c8cf 331
jah128 12:878c6e9d9e60 332 void Psiswarm::IF_update_encoders()
jah128 0:d6269d17c8cf 333 {
jah128 0:d6269d17c8cf 334 char rwep = previous_wheel_encoder_byte >> 2;
jah128 0:d6269d17c8cf 335 char rwe = wheel_encoder_byte >> 2;
jah128 0:d6269d17c8cf 336 char lwep = previous_wheel_encoder_byte % 4;
jah128 0:d6269d17c8cf 337 char lwe = wheel_encoder_byte % 4;
jah128 0:d6269d17c8cf 338 //pc.printf("L:%d P:%d R:%d P:%d \n",lwe,lwep,rwe,rwep);
jah128 0:d6269d17c8cf 339 if(lwe == 0 && lwep==1) left_encoder++;
jah128 0:d6269d17c8cf 340 if(lwe == 0 && lwep==2) left_encoder--;
jah128 0:d6269d17c8cf 341 if(rwe == 0 && rwep==1) right_encoder++;
jah128 0:d6269d17c8cf 342 if(rwe == 0 && rwep==2) right_encoder--;
jah128 0:d6269d17c8cf 343 if(left_encoder % 100 == 0) pc.printf("L:%d\n",left_encoder);
jah128 0:d6269d17c8cf 344 }
jah128 0:d6269d17c8cf 345
jah128 12:878c6e9d9e60 346 void Psiswarm::IF_update_user_id()
jah128 0:d6269d17c8cf 347 {
jah128 0:d6269d17c8cf 348 }
jah128 0:d6269d17c8cf 349
jah128 12:878c6e9d9e60 350 void Psiswarm::IF_update_switch()
jah128 0:d6269d17c8cf 351 {
jah128 0:d6269d17c8cf 352 // The user switch has changed state
jah128 0:d6269d17c8cf 353 // In this implementation we will only act on positive changes (rising edges)
jah128 0:d6269d17c8cf 354 // Subtracting new_state from (new_state & old_state) gives the positive changes
jah128 0:d6269d17c8cf 355 char positive_change = switch_byte - (switch_byte & previous_switch_byte);
jah128 11:312663037b8c 356 if(demo_on) demo.demo_handle_switch_event(positive_change);
jah128 0:d6269d17c8cf 357 else handle_switch_event(positive_change);
jah128 0:d6269d17c8cf 358 }
jah128 0:d6269d17c8cf 359
jah128 12:878c6e9d9e60 360 void Psiswarm::reset_encoders()
jah128 0:d6269d17c8cf 361 {
jah128 0:d6269d17c8cf 362 left_encoder = 0;
jah128 1:060690a934a9 363 right_encoder = 0;
jah128 0:d6269d17c8cf 364 }
jah128 0:d6269d17c8cf 365
jah128 12:878c6e9d9e60 366 void Psiswarm::debug(const char* format, ...)
jah128 0:d6269d17c8cf 367 {
jah128 1:060690a934a9 368 char buffer[256];
jah128 1:060690a934a9 369 if (debug_mode) {
jah128 1:060690a934a9 370 va_list vl;
jah128 1:060690a934a9 371 va_start(vl, format);
jah128 1:060690a934a9 372 vsprintf(buffer,format,vl);
jah128 1:060690a934a9 373 if(debug_output & 2) bt.printf("%s", buffer);
jah128 1:060690a934a9 374 if(debug_output & 1) pc.printf("%s", buffer);
jah128 1:060690a934a9 375 if(debug_output & 4) display.debug_page(buffer,strlen(buffer));
jah128 1:060690a934a9 376 va_end(vl);
jah128 1:060690a934a9 377 }
jah128 0:d6269d17c8cf 378 }
jah128 0:d6269d17c8cf 379
jah128 12:878c6e9d9e60 380 float Psiswarm::get_uptime(void)
jah128 0:d6269d17c8cf 381 {
jah128 1:060690a934a9 382 return uptime.read() + (timer_minute_count * 60);
jah128 0:d6269d17c8cf 383 }
jah128 0:d6269d17c8cf 384
jah128 12:878c6e9d9e60 385 void Psiswarm::pause_user_code(float period)
jah128 1:060690a934a9 386 {
jah128 0:d6269d17c8cf 387 user_code_restore_mode = user_code_running;
jah128 0:d6269d17c8cf 388 user_code_running = 0;
richardredpath 20:2b6ebe60929d 389 pause_usercode_timeout.attach(callback(this, &Psiswarm::IF_end_pause_user_code), period);
jah128 0:d6269d17c8cf 390 }
jah128 0:d6269d17c8cf 391
jah128 12:878c6e9d9e60 392 void Psiswarm::IF_end_pause_user_code()
jah128 1:060690a934a9 393 {
jah128 1:060690a934a9 394 user_code_running = user_code_restore_mode;
jah128 1:060690a934a9 395 }
jah128 1:060690a934a9 396
jah128 12:878c6e9d9e60 397 void Psiswarm::IF_get_hardware_description()
jah128 1:060690a934a9 398 {
jah128 17:bf614e28668f 399 debug("- Robot serial number %1.2f\n",serial_number);
jah128 17:bf614e28668f 400 debug("- PCB version %1.2f\n",pcb_version);
jah128 17:bf614e28668f 401 debug("- Hardware: ");
jah128 17:bf614e28668f 402 if(has_compass) debug("COMPASS ");
jah128 17:bf614e28668f 403 if(has_side_ir) debug("SIDE-IR ");
jah128 17:bf614e28668f 404 if(has_base_ir) debug("BASE-IR ");
jah128 17:bf614e28668f 405 if(has_base_colour_sensor) debug("BASE-COLOUR ");
jah128 17:bf614e28668f 406 if(has_top_colour_sensor) debug("TOP-COLOUR ");
jah128 17:bf614e28668f 407 if(has_wheel_encoders) debug("WHEEL-ENC ");
jah128 17:bf614e28668f 408 if(has_audio_pic) debug("AUDIO ");
jah128 17:bf614e28668f 409 if(has_ultrasonic_sensor) debug("ULTRASONIC ");
jah128 17:bf614e28668f 410 if(has_temperature_sensor) debug("TEMPERATURE ");
jah128 17:bf614e28668f 411 if(has_recharging_circuit) debug("RECHARGING ");
jah128 17:bf614e28668f 412 if(has_433_radio) debug("433-RADIO");
jah128 17:bf614e28668f 413 debug("\n");
jah128 0:d6269d17c8cf 414 }