Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
psiswarm.cpp@17:bf614e28668f, 2017-06-04 (annotated)
- Committer:
- jah128
- Date:
- Sun Jun 04 13:11:09 2017 +0000
- Revision:
- 17:bf614e28668f
- Parent:
- 16:50686c07ad07
- Child:
- 18:9204f74069b4
Updated calibration menus, fixed some bugs in demo, create new store_line_position routine using calibrated values
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:1c621cb8cf0d | 111 | |
jah128 | 0:d6269d17c8cf | 112 | char debug_mode = DEBUG_MODE; |
jah128 | 0:d6269d17c8cf | 113 | char debug_output = DEBUG_OUTPUT_STREAM; |
jah128 | 1:060690a934a9 | 114 | |
jah128 | 17:bf614e28668f | 115 | char firmware_bytes[80]; |
jah128 | 0:d6269d17c8cf | 116 | |
jah128 | 0:d6269d17c8cf | 117 | int base_colour_sensor_raw_values [4]; |
jah128 | 0:d6269d17c8cf | 118 | int top_colour_sensor_raw_values [4]; |
jah128 | 0:d6269d17c8cf | 119 | |
jah128 | 0:d6269d17c8cf | 120 | char waiting_for_ultrasonic = 0; |
jah128 | 0:d6269d17c8cf | 121 | int ultrasonic_distance = 0; |
jah128 | 0:d6269d17c8cf | 122 | char ultrasonic_distance_updated = 0; |
jah128 | 0:d6269d17c8cf | 123 | |
jah128 | 0:d6269d17c8cf | 124 | |
jah128 | 0:d6269d17c8cf | 125 | float line_position = 0; |
jah128 | 0:d6269d17c8cf | 126 | char line_found = 0; |
jah128 | 0:d6269d17c8cf | 127 | |
jah128 | 0:d6269d17c8cf | 128 | unsigned short background_ir_values [8]; |
jah128 | 0:d6269d17c8cf | 129 | unsigned short illuminated_ir_values [8]; |
jah128 | 0:d6269d17c8cf | 130 | float reflected_ir_distances [8]; |
jah128 | 0:d6269d17c8cf | 131 | char ir_values_stored = 0; |
jah128 | 0:d6269d17c8cf | 132 | unsigned short background_base_ir_values [5]; |
jah128 | 0:d6269d17c8cf | 133 | unsigned short illuminated_base_ir_values [5]; |
jah128 | 0:d6269d17c8cf | 134 | char base_ir_values_stored = 0; |
jah128 | 0:d6269d17c8cf | 135 | |
jah128 | 0:d6269d17c8cf | 136 | float motor_left_speed; |
jah128 | 0:d6269d17c8cf | 137 | float motor_right_speed; |
jah128 | 0:d6269d17c8cf | 138 | char motor_left_brake; |
jah128 | 0:d6269d17c8cf | 139 | char motor_right_brake; |
jah128 | 0:d6269d17c8cf | 140 | |
jah128 | 0:d6269d17c8cf | 141 | char demo_on = 0; |
jah128 | 0:d6269d17c8cf | 142 | char event = 0; |
jah128 | 0:d6269d17c8cf | 143 | char change_id_event = 0; |
jah128 | 0:d6269d17c8cf | 144 | char encoder_event = 0; |
jah128 | 0:d6269d17c8cf | 145 | char switch_event = 0; |
jah128 | 0:d6269d17c8cf | 146 | char user_code_running = 0; |
jah128 | 0:d6269d17c8cf | 147 | char user_code_restore_mode = 0; |
jah128 | 0:d6269d17c8cf | 148 | char system_warnings = 0; |
jah128 | 0:d6269d17c8cf | 149 | |
jah128 | 0:d6269d17c8cf | 150 | vector<string> basic_filenames; //filenames are stored in a vector string |
jah128 | 0:d6269d17c8cf | 151 | char psi_basic_file_count = 0; |
jah128 | 0:d6269d17c8cf | 152 | char use_flash_basic = 0; |
jah128 | 0:d6269d17c8cf | 153 | char file_transfer_mode = 0; |
jah128 | 0:d6269d17c8cf | 154 | |
jah128 | 0:d6269d17c8cf | 155 | int ir_pulse_delay = DEFAULT_IR_PULSE_DELAY; |
jah128 | 0:d6269d17c8cf | 156 | int base_ir_pulse_delay = DEFAULT_BASE_IR_PULSE_DELAY; |
jah128 | 0:d6269d17c8cf | 157 | |
jah128 | 0:d6269d17c8cf | 158 | /** |
jah128 | 0:d6269d17c8cf | 159 | * init() |
jah128 | 0:d6269d17c8cf | 160 | * |
jah128 | 0:d6269d17c8cf | 161 | * Main initialisation routine for the PsiSwarm robot |
jah128 | 0:d6269d17c8cf | 162 | * |
jah128 | 0:d6269d17c8cf | 163 | * Set up the GPIO expansion ICs, launch demo mode if button is held |
jah128 | 0:d6269d17c8cf | 164 | */ |
jah128 | 12:878c6e9d9e60 | 165 | void Psiswarm::init() |
jah128 | 0:d6269d17c8cf | 166 | { |
jah128 | 0:d6269d17c8cf | 167 | firmware_version=0; |
jah128 | 0:d6269d17c8cf | 168 | timer_minute_count = 0; |
jah128 | 12:878c6e9d9e60 | 169 | timer_ticker.attach(this,&Psiswarm::IF_update_minutes, 300); |
jah128 | 0:d6269d17c8cf | 170 | uptime.start(); |
jah128 | 0:d6269d17c8cf | 171 | primary_i2c.frequency(400000); |
jah128 | 11:312663037b8c | 172 | serial.setup_serial_interfaces(); |
jah128 | 17:bf614e28668f | 173 | debug("\n_________________________________________________________________________\n"); |
jah128 | 17:bf614e28668f | 174 | debug("--------------------PsiSwarm Robot Library %1.2f--------------------------\n",SOFTWARE_VERSION_CODE); |
jah128 | 17:bf614e28668f | 175 | debug("_________________________________________________________________________\n"); |
jah128 | 17:bf614e28668f | 176 | debug("- Setting up serial interface\n"); |
jah128 | 15:66be5ec52c3b | 177 | debug("- Set up display\n"); |
jah128 | 15:66be5ec52c3b | 178 | display.init_display_start(); |
jah128 | 17:bf614e28668f | 179 | debug("- Setting up GPIO expansion\n"); |
jah128 | 17:bf614e28668f | 180 | i2c_setup.IF_setup_gpio_expansion_ic(); |
jah128 | 0:d6269d17c8cf | 181 | debug("- Reading firmware: "); |
jah128 | 8:6c92789d5f87 | 182 | if(eprom.read_firmware() == 1) { |
jah128 | 1:060690a934a9 | 183 | debug("Version %3.2f\n",firmware_version); |
jah128 | 1:060690a934a9 | 184 | IF_get_hardware_description(); |
jah128 | 17:bf614e28668f | 185 | if(use_motor_calibration) { |
jah128 | 17:bf614e28668f | 186 | if(!motor_calibration_set) { |
jah128 | 17:bf614e28668f | 187 | if(firmware_version < 1.1) { |
jah128 | 17:bf614e28668f | 188 | debug("- WARNING: Firmware incompatible with motor calibration\n"); |
jah128 | 15:66be5ec52c3b | 189 | debug("- WARNING: Please update the firmware to use this feature.\n"); |
jah128 | 17:bf614e28668f | 190 | use_motor_calibration = 0; |
jah128 | 17:bf614e28668f | 191 | } else { |
jah128 | 15:66be5ec52c3b | 192 | debug("- WARNING: Motor calibration values have not been stored in firmware\n"); |
jah128 | 17:bf614e28668f | 193 | debug("- WARNING: Run motor calibration routine to use this feature.\n"); |
jah128 | 17:bf614e28668f | 194 | use_motor_calibration = 0; |
jah128 | 4:1c621cb8cf0d | 195 | } |
jah128 | 17:bf614e28668f | 196 | } else { |
jah128 | 17:bf614e28668f | 197 | debug("- Motor calibration in use [LEFT:%0.4f RIGHT:%0.4f]",left_motor_calibration_value,right_motor_calibration_value); |
jah128 | 4:1c621cb8cf0d | 198 | } |
jah128 | 4:1c621cb8cf0d | 199 | } |
jah128 | 17:bf614e28668f | 200 | if(base_ir_calibration_set != 1) { |
jah128 | 17:bf614e28668f | 201 | if(firmware_version < 1.2) { |
jah128 | 17:bf614e28668f | 202 | debug("- WARNING: Firmware incompatible with base IR sensor calibration\n"); |
jah128 | 17:bf614e28668f | 203 | debug("- WARNING: Please update the firmware to use this feature.\n"); |
jah128 | 17:bf614e28668f | 204 | } else { |
jah128 | 17:bf614e28668f | 205 | debug("- WARNING: Base IR calibration values not stored in firmware\n"); |
jah128 | 17:bf614e28668f | 206 | debug("- WARNING: Run sensor calibration routine to use this feature.\n"); |
jah128 | 17:bf614e28668f | 207 | } |
jah128 | 17:bf614e28668f | 208 | // Set default calibration values for base IR sensor |
jah128 | 17:bf614e28668f | 209 | sensors.IF_set_base_calibration_values(BIR1W,BIR2W,BIR3W,BIR4W,BIR5W,BIR1B,BIR2B,BIR3B,BIR4B,BIR5B); |
jah128 | 17:bf614e28668f | 210 | } else debug("- Using base IR calibration values stored in firmware\n"); |
jah128 | 17:bf614e28668f | 211 | if(has_base_colour_sensor == 1 && base_colour_calibration_set != 1){ |
jah128 | 17:bf614e28668f | 212 | if(firmware_version < 1.2) { |
jah128 | 17:bf614e28668f | 213 | debug("- WARNING: This firmware is incompatible with base colour sensor calibration\n"); |
jah128 | 17:bf614e28668f | 214 | debug("- WARNING: Please update the firmware to use this feature.\n"); |
jah128 | 17:bf614e28668f | 215 | } else { |
jah128 | 17:bf614e28668f | 216 | debug("- WARNING: Base colour sensor calibration values not stored in firmware\n"); |
jah128 | 17:bf614e28668f | 217 | debug("- WARNING: Run sensor calibration routine to use this feature.\n"); |
jah128 | 17:bf614e28668f | 218 | } |
jah128 | 17:bf614e28668f | 219 | // Set default calibration values for base colour sensor |
jah128 | 17:bf614e28668f | 220 | 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 | 17:bf614e28668f | 221 | } else debug("- Using base colour sensor calibration values stored in firmware\n"); |
jah128 | 17:bf614e28668f | 222 | if(firmware_version < TARGET_FIRMWARE_VERSION && AUTO_UPDATE_FIRMWARE == 1) eprom.update_firmware(); |
jah128 | 1:060690a934a9 | 223 | } else { |
jah128 | 1:060690a934a9 | 224 | debug("INVALID\n"); |
jah128 | 17:bf614e28668f | 225 | debug("- WARNING: Check firmware to enable robot features\n"); |
jah128 | 1:060690a934a9 | 226 | } |
jah128 | 17:bf614e28668f | 227 | |
jah128 | 17:bf614e28668f | 228 | |
jah128 | 17:bf614e28668f | 229 | |
jah128 | 1:060690a934a9 | 230 | if(ENABLE_BASIC == 1) { |
jah128 | 12:878c6e9d9e60 | 231 | basic.read_list_of_file_names(); |
jah128 | 1:060690a934a9 | 232 | if(psi_basic_file_count == 0) { |
jah128 | 1:060690a934a9 | 233 | debug("- No PsiBasic files found\n"); |
jah128 | 1:060690a934a9 | 234 | } else use_flash_basic = 1; |
jah128 | 0:d6269d17c8cf | 235 | } |
jah128 | 0:d6269d17c8cf | 236 | debug("- Setting up PIC microcontroller\n"); |
jah128 | 0:d6269d17c8cf | 237 | // IF_check_pic_firmware(); |
jah128 | 0:d6269d17c8cf | 238 | debug("- Setting up LED drivers\n"); |
jah128 | 9:dde9e21030eb | 239 | led.IF_init_leds(); |
jah128 | 11:312663037b8c | 240 | if(i2c_setup.IF_setup_led_expansion_ic() != 0) { |
jah128 | 1:060690a934a9 | 241 | debug("- WARNING: No I2C acknowledge for LED driver\n"); |
jah128 | 1:060690a934a9 | 242 | system_warnings += 1; |
jah128 | 0:d6269d17c8cf | 243 | } |
jah128 | 0:d6269d17c8cf | 244 | debug("- Setting up motor drivers\n"); |
jah128 | 8:6c92789d5f87 | 245 | motors.init_motors(); |
jah128 | 0:d6269d17c8cf | 246 | reset_encoders(); |
jah128 | 1:060690a934a9 | 247 | if(has_temperature_sensor) { |
jah128 | 1:060690a934a9 | 248 | debug("- Setting up temperature sensor\n"); |
jah128 | 11:312663037b8c | 249 | i2c_setup.IF_setup_temperature_sensor(); |
jah128 | 1:060690a934a9 | 250 | } |
jah128 | 1:060690a934a9 | 251 | if(has_base_colour_sensor) { |
jah128 | 1:060690a934a9 | 252 | debug("- Setting up base colour sensor\n"); |
jah128 | 17:bf614e28668f | 253 | if(colour.IF_check_base_colour_sensor() == 1) { |
jah128 | 17:bf614e28668f | 254 | colour.colour_sensor_init(); |
jah128 | 17:bf614e28668f | 255 | } else debug("- WARNING: Invalid response from colour sensor"); |
jah128 | 1:060690a934a9 | 256 | } |
jah128 | 1:060690a934a9 | 257 | if(has_ultrasonic_sensor) { |
jah128 | 1:060690a934a9 | 258 | debug("- Setting up ultrasonic sensor\n"); |
jah128 | 1:060690a934a9 | 259 | //enable_ultrasonic_ticker(); |
jah128 | 1:060690a934a9 | 260 | } |
jah128 | 1:060690a934a9 | 261 | |
jah128 | 0:d6269d17c8cf | 262 | debug("- Robot ID: %d\n",robot_id); |
jah128 | 11:312663037b8c | 263 | char switchstate = i2c_setup.IF_get_switch_state(); |
jah128 | 0:d6269d17c8cf | 264 | debug("- Switch State : %d\n",switchstate); |
jah128 | 10:e58323951c08 | 265 | debug("- Battery Voltage: %1.3fV\n",sensors.get_battery_voltage()); |
jah128 | 10:e58323951c08 | 266 | debug("- DC Voltage : %1.3fV\n",sensors.get_dc_voltage()); |
jah128 | 10:e58323951c08 | 267 | debug("- Current Draw : %1.3fA\n",sensors.get_current()); |
jah128 | 17:bf614e28668f | 268 | if(has_temperature_sensor) { |
jah128 | 17:bf614e28668f | 269 | debug("- Temperature : %1.3fC\n",sensors.get_temperature()); |
jah128 | 1:060690a934a9 | 270 | } |
jah128 | 11:312663037b8c | 271 | char demo_on = 0; |
jah128 | 11:312663037b8c | 272 | if(ENABLE_DEMO == 1 && switchstate > 0) demo_on=1; |
jah128 | 15:66be5ec52c3b | 273 | display.init_display_end(demo_on); |
jah128 | 12:878c6e9d9e60 | 274 | event_handler.attach_us(this,&Psiswarm::IF_handle_events, 1000); |
jah128 | 11:312663037b8c | 275 | if(demo_on > 0) { |
jah128 | 0:d6269d17c8cf | 276 | debug("- Demo mode button is pressed\n"); |
jah128 | 15:66be5ec52c3b | 277 | wait(0.6); |
jah128 | 11:312663037b8c | 278 | demo_on = i2c_setup.IF_get_switch_state(); |
jah128 | 11:312663037b8c | 279 | if(demo_on > 0) demo.start_demo_mode(); |
jah128 | 15:66be5ec52c3b | 280 | display.init_display_end(0); |
jah128 | 0:d6269d17c8cf | 281 | } |
jah128 | 17:bf614e28668f | 282 | debug("_________________________________________________________________________\n\n"); |
jah128 | 0:d6269d17c8cf | 283 | } |
jah128 | 0:d6269d17c8cf | 284 | |
jah128 | 12:878c6e9d9e60 | 285 | void Psiswarm::IF_update_minutes() |
jah128 | 1:060690a934a9 | 286 | { |
jah128 | 0:d6269d17c8cf | 287 | uptime.reset(); |
jah128 | 0:d6269d17c8cf | 288 | timer_minute_count += 5; |
jah128 | 0:d6269d17c8cf | 289 | } |
jah128 | 0:d6269d17c8cf | 290 | |
jah128 | 12:878c6e9d9e60 | 291 | void Psiswarm::IF_handle_events() |
jah128 | 0:d6269d17c8cf | 292 | { |
jah128 | 0:d6269d17c8cf | 293 | // This is the main 'operating system' thread that handles events from robot stimuli |
jah128 | 0:d6269d17c8cf | 294 | // By default it is run every 1ms and checks if there are events to handle |
jah128 | 1:060690a934a9 | 295 | if(event > 0) { |
jah128 | 0:d6269d17c8cf | 296 | // 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 | 297 | if(encoder_event == 1) { |
jah128 | 0:d6269d17c8cf | 298 | // The encoders have changed; update the encoder values |
jah128 | 0:d6269d17c8cf | 299 | IF_update_encoders(); |
jah128 | 1:060690a934a9 | 300 | encoder_event = 0; |
jah128 | 1:060690a934a9 | 301 | event--; |
jah128 | 0:d6269d17c8cf | 302 | } else { |
jah128 | 1:060690a934a9 | 303 | if(switch_event == 1) { |
jah128 | 1:060690a934a9 | 304 | IF_update_switch(); |
jah128 | 1:060690a934a9 | 305 | switch_event = 0; |
jah128 | 1:060690a934a9 | 306 | event--; |
jah128 | 0:d6269d17c8cf | 307 | } |
jah128 | 1:060690a934a9 | 308 | if(change_id_event == 1) { |
jah128 | 0:d6269d17c8cf | 309 | // The user ID for the robot has been changed |
jah128 | 0:d6269d17c8cf | 310 | IF_update_user_id(); |
jah128 | 0:d6269d17c8cf | 311 | change_id_event = 0; |
jah128 | 0:d6269d17c8cf | 312 | event--; |
jah128 | 0:d6269d17c8cf | 313 | } |
jah128 | 0:d6269d17c8cf | 314 | } |
jah128 | 1:060690a934a9 | 315 | } |
jah128 | 0:d6269d17c8cf | 316 | } |
jah128 | 0:d6269d17c8cf | 317 | |
jah128 | 12:878c6e9d9e60 | 318 | void Psiswarm::IF_update_encoders() |
jah128 | 0:d6269d17c8cf | 319 | { |
jah128 | 0:d6269d17c8cf | 320 | char rwep = previous_wheel_encoder_byte >> 2; |
jah128 | 0:d6269d17c8cf | 321 | char rwe = wheel_encoder_byte >> 2; |
jah128 | 0:d6269d17c8cf | 322 | char lwep = previous_wheel_encoder_byte % 4; |
jah128 | 0:d6269d17c8cf | 323 | char lwe = wheel_encoder_byte % 4; |
jah128 | 0:d6269d17c8cf | 324 | //pc.printf("L:%d P:%d R:%d P:%d \n",lwe,lwep,rwe,rwep); |
jah128 | 0:d6269d17c8cf | 325 | if(lwe == 0 && lwep==1) left_encoder++; |
jah128 | 0:d6269d17c8cf | 326 | if(lwe == 0 && lwep==2) left_encoder--; |
jah128 | 0:d6269d17c8cf | 327 | if(rwe == 0 && rwep==1) right_encoder++; |
jah128 | 0:d6269d17c8cf | 328 | if(rwe == 0 && rwep==2) right_encoder--; |
jah128 | 0:d6269d17c8cf | 329 | if(left_encoder % 100 == 0) pc.printf("L:%d\n",left_encoder); |
jah128 | 0:d6269d17c8cf | 330 | } |
jah128 | 0:d6269d17c8cf | 331 | |
jah128 | 12:878c6e9d9e60 | 332 | void Psiswarm::IF_update_user_id() |
jah128 | 0:d6269d17c8cf | 333 | { |
jah128 | 0:d6269d17c8cf | 334 | } |
jah128 | 0:d6269d17c8cf | 335 | |
jah128 | 12:878c6e9d9e60 | 336 | void Psiswarm::IF_update_switch() |
jah128 | 0:d6269d17c8cf | 337 | { |
jah128 | 0:d6269d17c8cf | 338 | // The user switch has changed state |
jah128 | 0:d6269d17c8cf | 339 | // In this implementation we will only act on positive changes (rising edges) |
jah128 | 0:d6269d17c8cf | 340 | // Subtracting new_state from (new_state & old_state) gives the positive changes |
jah128 | 0:d6269d17c8cf | 341 | char positive_change = switch_byte - (switch_byte & previous_switch_byte); |
jah128 | 11:312663037b8c | 342 | if(demo_on) demo.demo_handle_switch_event(positive_change); |
jah128 | 0:d6269d17c8cf | 343 | else handle_switch_event(positive_change); |
jah128 | 0:d6269d17c8cf | 344 | } |
jah128 | 0:d6269d17c8cf | 345 | |
jah128 | 12:878c6e9d9e60 | 346 | void Psiswarm::reset_encoders() |
jah128 | 0:d6269d17c8cf | 347 | { |
jah128 | 0:d6269d17c8cf | 348 | left_encoder = 0; |
jah128 | 1:060690a934a9 | 349 | right_encoder = 0; |
jah128 | 0:d6269d17c8cf | 350 | } |
jah128 | 0:d6269d17c8cf | 351 | |
jah128 | 12:878c6e9d9e60 | 352 | void Psiswarm::debug(const char* format, ...) |
jah128 | 0:d6269d17c8cf | 353 | { |
jah128 | 1:060690a934a9 | 354 | char buffer[256]; |
jah128 | 1:060690a934a9 | 355 | if (debug_mode) { |
jah128 | 1:060690a934a9 | 356 | va_list vl; |
jah128 | 1:060690a934a9 | 357 | va_start(vl, format); |
jah128 | 1:060690a934a9 | 358 | vsprintf(buffer,format,vl); |
jah128 | 1:060690a934a9 | 359 | if(debug_output & 2) bt.printf("%s", buffer); |
jah128 | 1:060690a934a9 | 360 | if(debug_output & 1) pc.printf("%s", buffer); |
jah128 | 1:060690a934a9 | 361 | if(debug_output & 4) display.debug_page(buffer,strlen(buffer)); |
jah128 | 1:060690a934a9 | 362 | va_end(vl); |
jah128 | 1:060690a934a9 | 363 | } |
jah128 | 0:d6269d17c8cf | 364 | } |
jah128 | 0:d6269d17c8cf | 365 | |
jah128 | 12:878c6e9d9e60 | 366 | float Psiswarm::get_uptime(void) |
jah128 | 0:d6269d17c8cf | 367 | { |
jah128 | 1:060690a934a9 | 368 | return uptime.read() + (timer_minute_count * 60); |
jah128 | 0:d6269d17c8cf | 369 | } |
jah128 | 0:d6269d17c8cf | 370 | |
jah128 | 12:878c6e9d9e60 | 371 | void Psiswarm::pause_user_code(float period) |
jah128 | 1:060690a934a9 | 372 | { |
jah128 | 0:d6269d17c8cf | 373 | user_code_restore_mode = user_code_running; |
jah128 | 0:d6269d17c8cf | 374 | user_code_running = 0; |
jah128 | 12:878c6e9d9e60 | 375 | pause_usercode_timeout.attach(this,&Psiswarm::IF_end_pause_user_code, period); |
jah128 | 0:d6269d17c8cf | 376 | } |
jah128 | 0:d6269d17c8cf | 377 | |
jah128 | 12:878c6e9d9e60 | 378 | void Psiswarm::IF_end_pause_user_code() |
jah128 | 1:060690a934a9 | 379 | { |
jah128 | 1:060690a934a9 | 380 | user_code_running = user_code_restore_mode; |
jah128 | 1:060690a934a9 | 381 | } |
jah128 | 1:060690a934a9 | 382 | |
jah128 | 12:878c6e9d9e60 | 383 | void Psiswarm::IF_get_hardware_description() |
jah128 | 1:060690a934a9 | 384 | { |
jah128 | 17:bf614e28668f | 385 | debug("- Robot serial number %1.2f\n",serial_number); |
jah128 | 17:bf614e28668f | 386 | debug("- PCB version %1.2f\n",pcb_version); |
jah128 | 17:bf614e28668f | 387 | debug("- Hardware: "); |
jah128 | 17:bf614e28668f | 388 | if(has_compass) debug("COMPASS "); |
jah128 | 17:bf614e28668f | 389 | if(has_side_ir) debug("SIDE-IR "); |
jah128 | 17:bf614e28668f | 390 | if(has_base_ir) debug("BASE-IR "); |
jah128 | 17:bf614e28668f | 391 | if(has_base_colour_sensor) debug("BASE-COLOUR "); |
jah128 | 17:bf614e28668f | 392 | if(has_top_colour_sensor) debug("TOP-COLOUR "); |
jah128 | 17:bf614e28668f | 393 | if(has_wheel_encoders) debug("WHEEL-ENC "); |
jah128 | 17:bf614e28668f | 394 | if(has_audio_pic) debug("AUDIO "); |
jah128 | 17:bf614e28668f | 395 | if(has_ultrasonic_sensor) debug("ULTRASONIC "); |
jah128 | 17:bf614e28668f | 396 | if(has_temperature_sensor) debug("TEMPERATURE "); |
jah128 | 17:bf614e28668f | 397 | if(has_recharging_circuit) debug("RECHARGING "); |
jah128 | 17:bf614e28668f | 398 | if(has_433_radio) debug("433-RADIO"); |
jah128 | 17:bf614e28668f | 399 | debug("\n"); |
jah128 | 0:d6269d17c8cf | 400 | } |