Practical Robotics Modular Robot Library
robot.cpp@1:a6728adaf7e7, 2016-11-26 (annotated)
- Committer:
- jah128
- Date:
- Sat Nov 26 21:43:52 2016 +0000
- Revision:
- 1:a6728adaf7e7
- Parent:
- 0:8a2dd255c508
- Child:
- 2:bf34b86aa0f3
Added docs, debug
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jah128 | 0:8a2dd255c508 | 1 | #include "robot.h" |
jah128 | 0:8a2dd255c508 | 2 | |
jah128 | 0:8a2dd255c508 | 3 | Timer mbed_uptime; |
jah128 | 0:8a2dd255c508 | 4 | int timer_minute_count; |
jah128 | 0:8a2dd255c508 | 5 | Ticker timer_ticker; |
jah128 | 0:8a2dd255c508 | 6 | volatile char i2c_lock = 0; |
jah128 | 0:8a2dd255c508 | 7 | |
jah128 | 1:a6728adaf7e7 | 8 | char debug_mode = DEBUG_MODE; |
jah128 | 1:a6728adaf7e7 | 9 | char debug_output = DEBUG_OUTPUT_STREAM; |
jah128 | 1:a6728adaf7e7 | 10 | |
jah128 | 0:8a2dd255c508 | 11 | Led led; |
jah128 | 0:8a2dd255c508 | 12 | Sensors sensors; |
jah128 | 0:8a2dd255c508 | 13 | Motors motors; |
jah128 | 0:8a2dd255c508 | 14 | I2C primary_i2c (p9, p10); |
jah128 | 0:8a2dd255c508 | 15 | AnalogIn vin_battery(p17); |
jah128 | 0:8a2dd255c508 | 16 | Serial pc(USBTX,USBRX); |
jah128 | 1:a6728adaf7e7 | 17 | Serial bt(p14,p13); |
jah128 | 0:8a2dd255c508 | 18 | |
jah128 | 0:8a2dd255c508 | 19 | |
jah128 | 0:8a2dd255c508 | 20 | // Public functions |
jah128 | 0:8a2dd255c508 | 21 | |
jah128 | 0:8a2dd255c508 | 22 | void Robot::init(){ |
jah128 | 0:8a2dd255c508 | 23 | //Start the uptime timer |
jah128 | 0:8a2dd255c508 | 24 | timer_minute_count = 0; |
jah128 | 0:8a2dd255c508 | 25 | timer_ticker.attach(this,&Robot::_update_minutes, 300); //Update minutes resets the uptime timer every 5 minutes to prevent stop overruns |
jah128 | 0:8a2dd255c508 | 26 | mbed_uptime.start(); |
jah128 | 0:8a2dd255c508 | 27 | |
jah128 | 1:a6728adaf7e7 | 28 | //Setup serial interfaces |
jah128 | 1:a6728adaf7e7 | 29 | setup_serial_interfaces(); |
jah128 | 1:a6728adaf7e7 | 30 | debug("Practical Robotics Modular Robot\n"); |
jah128 | 1:a6728adaf7e7 | 31 | |
jah128 | 0:8a2dd255c508 | 32 | //Setup the primary i2c bus to 400kHz baud rate |
jah128 | 0:8a2dd255c508 | 33 | primary_i2c.frequency(400000); |
jah128 | 0:8a2dd255c508 | 34 | |
jah128 | 0:8a2dd255c508 | 35 | //Send a reset signal to the LED driver |
jah128 | 1:a6728adaf7e7 | 36 | debug("Resetting LED driver : "); |
jah128 | 1:a6728adaf7e7 | 37 | if(led.reset_led_driver() != 0)debug("FAILED\n"); |
jah128 | 1:a6728adaf7e7 | 38 | else debug("SUCCESS\n"); |
jah128 | 0:8a2dd255c508 | 39 | wait(0.05); |
jah128 | 0:8a2dd255c508 | 40 | |
jah128 | 0:8a2dd255c508 | 41 | //Send the initialisation signal to the LED driver |
jah128 | 1:a6728adaf7e7 | 42 | debug("Initialising LED driver : "); |
jah128 | 1:a6728adaf7e7 | 43 | if(led.init_led_driver() != 0)debug("FAILED\n"); |
jah128 | 1:a6728adaf7e7 | 44 | else debug("SUCCESS\n"); |
jah128 | 0:8a2dd255c508 | 45 | |
jah128 | 0:8a2dd255c508 | 46 | wait(0.05); |
jah128 | 0:8a2dd255c508 | 47 | |
jah128 | 0:8a2dd255c508 | 48 | //Start the Sharp sensor ticker |
jah128 | 1:a6728adaf7e7 | 49 | debug("Starting distance sensor ticker...\n"); |
jah128 | 0:8a2dd255c508 | 50 | sensors.start_sensor_ticker(); |
jah128 | 0:8a2dd255c508 | 51 | |
jah128 | 0:8a2dd255c508 | 52 | wait(0.05); |
jah128 | 0:8a2dd255c508 | 53 | |
jah128 | 0:8a2dd255c508 | 54 | //Setup the h-bridge drivers |
jah128 | 1:a6728adaf7e7 | 55 | debug("Starting the H-bridge drivers...\n"); |
jah128 | 0:8a2dd255c508 | 56 | motors.init(); |
jah128 | 0:8a2dd255c508 | 57 | } |
jah128 | 0:8a2dd255c508 | 58 | |
jah128 | 0:8a2dd255c508 | 59 | float Robot::get_uptime(void) |
jah128 | 0:8a2dd255c508 | 60 | { |
jah128 | 0:8a2dd255c508 | 61 | return mbed_uptime.read() + (timer_minute_count * 60); |
jah128 | 0:8a2dd255c508 | 62 | } |
jah128 | 0:8a2dd255c508 | 63 | |
jah128 | 0:8a2dd255c508 | 64 | float Robot::get_battery_voltage(void) |
jah128 | 0:8a2dd255c508 | 65 | { |
jah128 | 0:8a2dd255c508 | 66 | // Voltage is measured through a 7.5V zener diode followed by a 1:1 potential divider |
jah128 | 0:8a2dd255c508 | 67 | |
jah128 | 0:8a2dd255c508 | 68 | float measure_voltage = ZENER_VOLTAGE + (vin_battery.read() * BATTERY_PD_MULTIPLIER); |
jah128 | 0:8a2dd255c508 | 69 | return measure_voltage; |
jah128 | 0:8a2dd255c508 | 70 | } |
jah128 | 0:8a2dd255c508 | 71 | |
jah128 | 1:a6728adaf7e7 | 72 | void Robot::debug(const char* format, ...) |
jah128 | 1:a6728adaf7e7 | 73 | { |
jah128 | 1:a6728adaf7e7 | 74 | char buffer[256]; |
jah128 | 1:a6728adaf7e7 | 75 | if (debug_mode) { |
jah128 | 1:a6728adaf7e7 | 76 | va_list vl; |
jah128 | 1:a6728adaf7e7 | 77 | va_start(vl, format); |
jah128 | 1:a6728adaf7e7 | 78 | vsprintf(buffer,format,vl); |
jah128 | 1:a6728adaf7e7 | 79 | if(debug_output & 2) bt.printf("%s", buffer); |
jah128 | 1:a6728adaf7e7 | 80 | if(debug_output & 1) pc.printf("%s", buffer); |
jah128 | 1:a6728adaf7e7 | 81 | va_end(vl); |
jah128 | 1:a6728adaf7e7 | 82 | } |
jah128 | 1:a6728adaf7e7 | 83 | } |
jah128 | 1:a6728adaf7e7 | 84 | |
jah128 | 1:a6728adaf7e7 | 85 | void Robot::setup_serial_interfaces() |
jah128 | 1:a6728adaf7e7 | 86 | { |
jah128 | 1:a6728adaf7e7 | 87 | if(ENABLE_PC_SERIAL) { |
jah128 | 1:a6728adaf7e7 | 88 | pc.baud(PC_BAUD); |
jah128 | 1:a6728adaf7e7 | 89 | pc.attach(this,&Robot::_pc_rx_callback, Serial::RxIrq); |
jah128 | 1:a6728adaf7e7 | 90 | } |
jah128 | 1:a6728adaf7e7 | 91 | if(ENABLE_BLUETOOTH) { |
jah128 | 1:a6728adaf7e7 | 92 | bt.baud(BLUETOOTH_BAUD); |
jah128 | 1:a6728adaf7e7 | 93 | bt.attach(this,&Robot::_bt_rx_callback, Serial::RxIrq); |
jah128 | 1:a6728adaf7e7 | 94 | } |
jah128 | 1:a6728adaf7e7 | 95 | } |
jah128 | 1:a6728adaf7e7 | 96 | |
jah128 | 0:8a2dd255c508 | 97 | // Private functions |
jah128 | 0:8a2dd255c508 | 98 | |
jah128 | 1:a6728adaf7e7 | 99 | void Robot::_pc_rx_callback() |
jah128 | 1:a6728adaf7e7 | 100 | { |
jah128 | 1:a6728adaf7e7 | 101 | |
jah128 | 1:a6728adaf7e7 | 102 | } |
jah128 | 1:a6728adaf7e7 | 103 | |
jah128 | 1:a6728adaf7e7 | 104 | void Robot::_bt_rx_callback() |
jah128 | 1:a6728adaf7e7 | 105 | { |
jah128 | 1:a6728adaf7e7 | 106 | |
jah128 | 1:a6728adaf7e7 | 107 | } |
jah128 | 1:a6728adaf7e7 | 108 | |
jah128 | 1:a6728adaf7e7 | 109 | |
jah128 | 0:8a2dd255c508 | 110 | void Robot::_update_minutes() |
jah128 | 0:8a2dd255c508 | 111 | { |
jah128 | 0:8a2dd255c508 | 112 | mbed_uptime.reset(); |
jah128 | 0:8a2dd255c508 | 113 | timer_minute_count += 5; |
jah128 | 0:8a2dd255c508 | 114 | } |