Psi Swarm Robot / PRRobot

Practical Robotics Modular Robot Library

Dependents:   ModularRobot

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?

UserRevisionLine numberNew 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 }