Practical Robotics Modular Robot Library
robot.cpp@6:732aa91eb555, 2017-01-13 (annotated)
- Committer:
- jah128
- Date:
- Fri Jan 13 23:16:23 2017 +0000
- Revision:
- 6:732aa91eb555
- Parent:
- 5:6da8daaeb9f7
Updated;
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 | 4:c2e933d53bea | 11 | SerialComms serial; |
jah128 | 0:8a2dd255c508 | 12 | Led led; |
jah128 | 0:8a2dd255c508 | 13 | Sensors sensors; |
jah128 | 0:8a2dd255c508 | 14 | Motors motors; |
jah128 | 2:bf34b86aa0f3 | 15 | |
jah128 | 2:bf34b86aa0f3 | 16 | Serial pc(USBTX,USBRX); |
jah128 | 4:c2e933d53bea | 17 | Serial bt(p13,p14); |
jah128 | 4:c2e933d53bea | 18 | |
jah128 | 3:8762f6b2ea8d | 19 | DigitalIn rpi1(p5,PullDown); |
jah128 | 0:8a2dd255c508 | 20 | I2C primary_i2c (p9, p10); |
jah128 | 2:bf34b86aa0f3 | 21 | DigitalOut case_led(p15); |
jah128 | 2:bf34b86aa0f3 | 22 | DigitalOut mbed_led1(LED1); |
jah128 | 2:bf34b86aa0f3 | 23 | DigitalOut mbed_led2(LED2); |
jah128 | 2:bf34b86aa0f3 | 24 | DigitalOut mbed_led3(LED3); |
jah128 | 2:bf34b86aa0f3 | 25 | DigitalOut mbed_led4(LED4); |
jah128 | 0:8a2dd255c508 | 26 | AnalogIn vin_battery(p17); |
jah128 | 4:c2e933d53bea | 27 | char status_message [8]; |
jah128 | 0:8a2dd255c508 | 28 | |
jah128 | 0:8a2dd255c508 | 29 | |
jah128 | 0:8a2dd255c508 | 30 | // Public functions |
jah128 | 0:8a2dd255c508 | 31 | |
jah128 | 4:c2e933d53bea | 32 | |
jah128 | 3:8762f6b2ea8d | 33 | void Robot::update_status_message(){ |
jah128 | 4:c2e933d53bea | 34 | // Compose an 8-character status message indicating the current status values |
jah128 | 3:8762f6b2ea8d | 35 | // First two bytes are uptime in 1/10ths of seconds |
jah128 | 3:8762f6b2ea8d | 36 | int current_uptime = (int) ((get_uptime() * 10.0)) % 65536; |
jah128 | 3:8762f6b2ea8d | 37 | status_message[0] = current_uptime / 256; |
jah128 | 3:8762f6b2ea8d | 38 | status_message[1] = current_uptime % 256; |
jah128 | 3:8762f6b2ea8d | 39 | // Next two bytes are battery voltage * 4096 |
jah128 | 3:8762f6b2ea8d | 40 | int current_voltage = (int) (get_battery_voltage() * 4096); |
jah128 | 3:8762f6b2ea8d | 41 | status_message[2] = current_voltage / 256; |
jah128 | 3:8762f6b2ea8d | 42 | status_message[3] = current_voltage % 256; |
jah128 | 3:8762f6b2ea8d | 43 | // Next byte is (left motor speed + 1) * 127 |
jah128 | 3:8762f6b2ea8d | 44 | status_message[4] = (int) ((motors.get_left_motor_speed() + 1.0) * 127.0); |
jah128 | 3:8762f6b2ea8d | 45 | // Next byte is (right motor speed + 1) * 127 |
jah128 | 3:8762f6b2ea8d | 46 | status_message[5] = (int) ((motors.get_right_motor_speed() + 1.0) * 127.0); |
jah128 | 3:8762f6b2ea8d | 47 | // Next byte is left motor current * 96 |
jah128 | 3:8762f6b2ea8d | 48 | status_message[6] = (int) (motors.get_current_left() * 96.0); |
jah128 | 3:8762f6b2ea8d | 49 | // Next byte is right motor current * 96 |
jah128 | 4:c2e933d53bea | 50 | status_message[7] = (int) (motors.get_current_right() * 96.0); |
jah128 | 4:c2e933d53bea | 51 | } |
jah128 | 4:c2e933d53bea | 52 | |
jah128 | 4:c2e933d53bea | 53 | void Robot::case_led_toggle(){ |
jah128 | 4:c2e933d53bea | 54 | case_led = 1-case_led; |
jah128 | 3:8762f6b2ea8d | 55 | } |
jah128 | 3:8762f6b2ea8d | 56 | |
jah128 | 0:8a2dd255c508 | 57 | void Robot::init(){ |
jah128 | 4:c2e933d53bea | 58 | //Flash the case LED during init routine |
jah128 | 4:c2e933d53bea | 59 | Ticker case_led_ticker; |
jah128 | 4:c2e933d53bea | 60 | case_led_ticker.attach(this,&Robot::case_led_toggle, 0.25); |
jah128 | 4:c2e933d53bea | 61 | |
jah128 | 0:8a2dd255c508 | 62 | //Start the uptime timer |
jah128 | 0:8a2dd255c508 | 63 | timer_minute_count = 0; |
jah128 | 0:8a2dd255c508 | 64 | timer_ticker.attach(this,&Robot::_update_minutes, 300); //Update minutes resets the uptime timer every 5 minutes to prevent stop overruns |
jah128 | 0:8a2dd255c508 | 65 | mbed_uptime.start(); |
jah128 | 0:8a2dd255c508 | 66 | |
jah128 | 1:a6728adaf7e7 | 67 | //Setup serial interfaces |
jah128 | 4:c2e933d53bea | 68 | serial.setup_serial_interfaces(); |
jah128 | 1:a6728adaf7e7 | 69 | debug("Practical Robotics Modular Robot\n"); |
jah128 | 1:a6728adaf7e7 | 70 | |
jah128 | 0:8a2dd255c508 | 71 | //Setup the primary i2c bus to 400kHz baud rate |
jah128 | 0:8a2dd255c508 | 72 | primary_i2c.frequency(400000); |
jah128 | 0:8a2dd255c508 | 73 | |
jah128 | 0:8a2dd255c508 | 74 | //Send a reset signal to the LED driver |
jah128 | 1:a6728adaf7e7 | 75 | debug("Resetting LED driver : "); |
jah128 | 1:a6728adaf7e7 | 76 | if(led.reset_led_driver() != 0)debug("FAILED\n"); |
jah128 | 1:a6728adaf7e7 | 77 | else debug("SUCCESS\n"); |
jah128 | 0:8a2dd255c508 | 78 | wait(0.05); |
jah128 | 0:8a2dd255c508 | 79 | |
jah128 | 0:8a2dd255c508 | 80 | //Send the initialisation signal to the LED driver |
jah128 | 1:a6728adaf7e7 | 81 | debug("Initialising LED driver : "); |
jah128 | 1:a6728adaf7e7 | 82 | if(led.init_led_driver() != 0)debug("FAILED\n"); |
jah128 | 1:a6728adaf7e7 | 83 | else debug("SUCCESS\n"); |
jah128 | 0:8a2dd255c508 | 84 | |
jah128 | 0:8a2dd255c508 | 85 | wait(0.05); |
jah128 | 0:8a2dd255c508 | 86 | |
jah128 | 0:8a2dd255c508 | 87 | //Start the Sharp sensor ticker |
jah128 | 1:a6728adaf7e7 | 88 | debug("Starting distance sensor ticker...\n"); |
jah128 | 0:8a2dd255c508 | 89 | sensors.start_sensor_ticker(); |
jah128 | 0:8a2dd255c508 | 90 | |
jah128 | 0:8a2dd255c508 | 91 | wait(0.05); |
jah128 | 0:8a2dd255c508 | 92 | |
jah128 | 0:8a2dd255c508 | 93 | //Setup the h-bridge drivers |
jah128 | 1:a6728adaf7e7 | 94 | debug("Starting the H-bridge drivers...\n"); |
jah128 | 0:8a2dd255c508 | 95 | motors.init(); |
jah128 | 3:8762f6b2ea8d | 96 | |
jah128 | 3:8762f6b2ea8d | 97 | wait(0.05); |
jah128 | 3:8762f6b2ea8d | 98 | |
jah128 | 3:8762f6b2ea8d | 99 | //Wait for R-Pi pin to become high |
jah128 | 3:8762f6b2ea8d | 100 | debug("Waiting for Raspberry Pi Zero...\n"); |
jah128 | 5:6da8daaeb9f7 | 101 | led.start_animation(0,3); |
jah128 | 3:8762f6b2ea8d | 102 | while(rpi1==0) wait_us(50); |
jah128 | 4:c2e933d53bea | 103 | led.stop_animation(); |
jah128 | 3:8762f6b2ea8d | 104 | led.all_off(); |
jah128 | 4:c2e933d53bea | 105 | case_led_ticker.detach(); |
jah128 | 4:c2e933d53bea | 106 | case_led = 1; |
jah128 | 5:6da8daaeb9f7 | 107 | led.start_animation(5,25); |
jah128 | 3:8762f6b2ea8d | 108 | debug("Initialisation finished: Starting program. \n"); |
jah128 | 0:8a2dd255c508 | 109 | } |
jah128 | 0:8a2dd255c508 | 110 | |
jah128 | 0:8a2dd255c508 | 111 | float Robot::get_uptime(void) |
jah128 | 0:8a2dd255c508 | 112 | { |
jah128 | 0:8a2dd255c508 | 113 | return mbed_uptime.read() + (timer_minute_count * 60); |
jah128 | 0:8a2dd255c508 | 114 | } |
jah128 | 0:8a2dd255c508 | 115 | |
jah128 | 0:8a2dd255c508 | 116 | float Robot::get_battery_voltage(void) |
jah128 | 0:8a2dd255c508 | 117 | { |
jah128 | 0:8a2dd255c508 | 118 | // Voltage is measured through a 7.5V zener diode followed by a 1:1 potential divider |
jah128 | 0:8a2dd255c508 | 119 | |
jah128 | 0:8a2dd255c508 | 120 | float measure_voltage = ZENER_VOLTAGE + (vin_battery.read() * BATTERY_PD_MULTIPLIER); |
jah128 | 0:8a2dd255c508 | 121 | return measure_voltage; |
jah128 | 0:8a2dd255c508 | 122 | } |
jah128 | 0:8a2dd255c508 | 123 | |
jah128 | 1:a6728adaf7e7 | 124 | void Robot::debug(const char* format, ...) |
jah128 | 1:a6728adaf7e7 | 125 | { |
jah128 | 1:a6728adaf7e7 | 126 | char buffer[256]; |
jah128 | 1:a6728adaf7e7 | 127 | if (debug_mode) { |
jah128 | 1:a6728adaf7e7 | 128 | va_list vl; |
jah128 | 1:a6728adaf7e7 | 129 | va_start(vl, format); |
jah128 | 1:a6728adaf7e7 | 130 | vsprintf(buffer,format,vl); |
jah128 | 1:a6728adaf7e7 | 131 | if(debug_output & 2) bt.printf("%s", buffer); |
jah128 | 1:a6728adaf7e7 | 132 | if(debug_output & 1) pc.printf("%s", buffer); |
jah128 | 1:a6728adaf7e7 | 133 | va_end(vl); |
jah128 | 1:a6728adaf7e7 | 134 | } |
jah128 | 1:a6728adaf7e7 | 135 | } |
jah128 | 1:a6728adaf7e7 | 136 | |
jah128 | 1:a6728adaf7e7 | 137 | |
jah128 | 1:a6728adaf7e7 | 138 | |
jah128 | 0:8a2dd255c508 | 139 | void Robot::_update_minutes() |
jah128 | 0:8a2dd255c508 | 140 | { |
jah128 | 0:8a2dd255c508 | 141 | mbed_uptime.reset(); |
jah128 | 0:8a2dd255c508 | 142 | timer_minute_count += 5; |
jah128 | 0:8a2dd255c508 | 143 | } |