Practical Robotics Modular Robot Library
robot.cpp
- Committer:
- jah128
- Date:
- 2016-11-28
- Revision:
- 3:8762f6b2ea8d
- Parent:
- 2:bf34b86aa0f3
- Child:
- 4:c2e933d53bea
File content as of revision 3:8762f6b2ea8d:
#include "robot.h" Timer mbed_uptime; int timer_minute_count; Ticker timer_ticker; volatile char i2c_lock = 0; char debug_mode = DEBUG_MODE; char debug_output = DEBUG_OUTPUT_STREAM; Led led; Sensors sensors; Motors motors; Serial pc(USBTX,USBRX); DigitalIn rpi1(p5,PullDown); I2C primary_i2c (p9, p10); Serial bt(p13,p14); DigitalOut case_led(p15); DigitalOut mbed_led1(LED1); DigitalOut mbed_led2(LED2); DigitalOut mbed_led3(LED3); DigitalOut mbed_led4(LED4); AnalogIn vin_battery(p17); char status_message [16]; // Public functions void Robot::update_status_message(){ // Compose a 16-byte status message indicating all the current sensor values // First two bytes are uptime in 1/10ths of seconds int current_uptime = (int) ((get_uptime() * 10.0)) % 65536; status_message[0] = current_uptime / 256; status_message[1] = current_uptime % 256; // Next two bytes are battery voltage * 4096 int current_voltage = (int) (get_battery_voltage() * 4096); status_message[2] = current_voltage / 256; status_message[3] = current_voltage % 256; // Next byte is (left motor speed + 1) * 127 status_message[4] = (int) ((motors.get_left_motor_speed() + 1.0) * 127.0); // Next byte is (right motor speed + 1) * 127 status_message[5] = (int) ((motors.get_right_motor_speed() + 1.0) * 127.0); // Next byte is left motor current * 96 status_message[6] = (int) (motors.get_current_left() * 96.0); // Next byte is right motor current * 96 status_message[7] = (int) (motors.get_current_right() * 96.0); for(int i=0;i<8;i++){ status_message[i+8] = sensors.get_adc_value(i); } } void Robot::init(){ //Start the uptime timer timer_minute_count = 0; timer_ticker.attach(this,&Robot::_update_minutes, 300); //Update minutes resets the uptime timer every 5 minutes to prevent stop overruns mbed_uptime.start(); //Setup serial interfaces setup_serial_interfaces(); debug("Practical Robotics Modular Robot\n"); //Setup the primary i2c bus to 400kHz baud rate primary_i2c.frequency(400000); //Send a reset signal to the LED driver debug("Resetting LED driver : "); if(led.reset_led_driver() != 0)debug("FAILED\n"); else debug("SUCCESS\n"); wait(0.05); //Send the initialisation signal to the LED driver debug("Initialising LED driver : "); if(led.init_led_driver() != 0)debug("FAILED\n"); else debug("SUCCESS\n"); wait(0.05); //Start the Sharp sensor ticker debug("Starting distance sensor ticker...\n"); sensors.start_sensor_ticker(); wait(0.05); //Setup the h-bridge drivers debug("Starting the H-bridge drivers...\n"); motors.init(); wait(0.05); //Wait for R-Pi pin to become high debug("Waiting for Raspberry Pi Zero...\n"); led.start_test(); while(rpi1==0) wait_us(50); led.all_off(); debug("Initialisation finished: Starting program. \n"); } float Robot::get_uptime(void) { return mbed_uptime.read() + (timer_minute_count * 60); } float Robot::get_battery_voltage(void) { // Voltage is measured through a 7.5V zener diode followed by a 1:1 potential divider float measure_voltage = ZENER_VOLTAGE + (vin_battery.read() * BATTERY_PD_MULTIPLIER); return measure_voltage; } void Robot::debug(const char* format, ...) { char buffer[256]; if (debug_mode) { va_list vl; va_start(vl, format); vsprintf(buffer,format,vl); if(debug_output & 2) bt.printf("%s", buffer); if(debug_output & 1) pc.printf("%s", buffer); va_end(vl); } } void Robot::setup_serial_interfaces() { if(ENABLE_PC_SERIAL) { pc.baud(PC_BAUD); pc.attach(this,&Robot::_pc_rx_callback, Serial::RxIrq); } if(ENABLE_BLUETOOTH) { bt.baud(BLUETOOTH_BAUD); bt.attach(this,&Robot::_bt_rx_callback, Serial::RxIrq); } } // Private functions void Robot::_pc_rx_callback() { } void Robot::_bt_rx_callback() { } void Robot::_update_minutes() { mbed_uptime.reset(); timer_minute_count += 5; }