Psi Swarm Robot / PRRobot

Practical Robotics Modular Robot Library

Dependents:   ModularRobot

robot.cpp@2:bf34b86aa0f3, 2016-11-26 (annotated)

Committer:
jah128
Date:
Sat Nov 26 21:50:10 2016 +0000
Revision:
2:bf34b86aa0f3
Parent:
1:a6728adaf7e7
Child:
3:8762f6b2ea8d
Added extra leds

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 2:bf34b86aa0f3 14
jah128 2:bf34b86aa0f3 15 Serial pc(USBTX,USBRX);
jah128 0:8a2dd255c508 16 I2C primary_i2c (p9, p10);
jah128 2:bf34b86aa0f3 17 Serial bt(p14,p13);
jah128 2:bf34b86aa0f3 18 DigitalOut case_led(p15);
jah128 2:bf34b86aa0f3 19 DigitalOut mbed_led1(LED1);
jah128 2:bf34b86aa0f3 20 DigitalOut mbed_led2(LED2);
jah128 2:bf34b86aa0f3 21 DigitalOut mbed_led3(LED3);
jah128 2:bf34b86aa0f3 22 DigitalOut mbed_led4(LED4);
jah128 0:8a2dd255c508 23 AnalogIn vin_battery(p17);
jah128 0:8a2dd255c508 24
jah128 0:8a2dd255c508 25
jah128 0:8a2dd255c508 26 // Public functions
jah128 0:8a2dd255c508 27
jah128 0:8a2dd255c508 28 void Robot::init(){
jah128 0:8a2dd255c508 29 //Start the uptime timer
jah128 0:8a2dd255c508 30 timer_minute_count = 0;
jah128 0:8a2dd255c508 31 timer_ticker.attach(this,&Robot::_update_minutes, 300); //Update minutes resets the uptime timer every 5 minutes to prevent stop overruns
jah128 0:8a2dd255c508 32 mbed_uptime.start();
jah128 0:8a2dd255c508 33
jah128 1:a6728adaf7e7 34 //Setup serial interfaces
jah128 1:a6728adaf7e7 35 setup_serial_interfaces();
jah128 1:a6728adaf7e7 36 debug("Practical Robotics Modular Robot\n");
jah128 1:a6728adaf7e7 37
jah128 0:8a2dd255c508 38 //Setup the primary i2c bus to 400kHz baud rate
jah128 0:8a2dd255c508 39 primary_i2c.frequency(400000);
jah128 0:8a2dd255c508 40
jah128 0:8a2dd255c508 41 //Send a reset signal to the LED driver
jah128 1:a6728adaf7e7 42 debug("Resetting LED driver : ");
jah128 1:a6728adaf7e7 43 if(led.reset_led_driver() != 0)debug("FAILED\n");
jah128 1:a6728adaf7e7 44 else debug("SUCCESS\n");
jah128 0:8a2dd255c508 45 wait(0.05);
jah128 0:8a2dd255c508 46
jah128 0:8a2dd255c508 47 //Send the initialisation signal to the LED driver
jah128 1:a6728adaf7e7 48 debug("Initialising LED driver : ");
jah128 1:a6728adaf7e7 49 if(led.init_led_driver() != 0)debug("FAILED\n");
jah128 1:a6728adaf7e7 50 else debug("SUCCESS\n");
jah128 0:8a2dd255c508 51
jah128 0:8a2dd255c508 52 wait(0.05);
jah128 0:8a2dd255c508 53
jah128 0:8a2dd255c508 54 //Start the Sharp sensor ticker
jah128 1:a6728adaf7e7 55 debug("Starting distance sensor ticker...\n");
jah128 0:8a2dd255c508 56 sensors.start_sensor_ticker();
jah128 0:8a2dd255c508 57
jah128 0:8a2dd255c508 58 wait(0.05);
jah128 0:8a2dd255c508 59
jah128 0:8a2dd255c508 60 //Setup the h-bridge drivers
jah128 1:a6728adaf7e7 61 debug("Starting the H-bridge drivers...\n");
jah128 0:8a2dd255c508 62 motors.init();
jah128 0:8a2dd255c508 63 }
jah128 0:8a2dd255c508 64
jah128 0:8a2dd255c508 65 float Robot::get_uptime(void)
jah128 0:8a2dd255c508 66 {
jah128 0:8a2dd255c508 67 return mbed_uptime.read() + (timer_minute_count * 60);
jah128 0:8a2dd255c508 68 }
jah128 0:8a2dd255c508 69
jah128 0:8a2dd255c508 70 float Robot::get_battery_voltage(void)
jah128 0:8a2dd255c508 71 {
jah128 0:8a2dd255c508 72 // Voltage is measured through a 7.5V zener diode followed by a 1:1 potential divider
jah128 0:8a2dd255c508 73
jah128 0:8a2dd255c508 74 float measure_voltage = ZENER_VOLTAGE + (vin_battery.read() * BATTERY_PD_MULTIPLIER);
jah128 0:8a2dd255c508 75 return measure_voltage;
jah128 0:8a2dd255c508 76 }
jah128 0:8a2dd255c508 77
jah128 1:a6728adaf7e7 78 void Robot::debug(const char* format, ...)
jah128 1:a6728adaf7e7 79 {
jah128 1:a6728adaf7e7 80 char buffer[256];
jah128 1:a6728adaf7e7 81 if (debug_mode) {
jah128 1:a6728adaf7e7 82 va_list vl;
jah128 1:a6728adaf7e7 83 va_start(vl, format);
jah128 1:a6728adaf7e7 84 vsprintf(buffer,format,vl);
jah128 1:a6728adaf7e7 85 if(debug_output & 2) bt.printf("%s", buffer);
jah128 1:a6728adaf7e7 86 if(debug_output & 1) pc.printf("%s", buffer);
jah128 1:a6728adaf7e7 87 va_end(vl);
jah128 1:a6728adaf7e7 88 }
jah128 1:a6728adaf7e7 89 }
jah128 1:a6728adaf7e7 90
jah128 1:a6728adaf7e7 91 void Robot::setup_serial_interfaces()
jah128 1:a6728adaf7e7 92 {
jah128 1:a6728adaf7e7 93 if(ENABLE_PC_SERIAL) {
jah128 1:a6728adaf7e7 94 pc.baud(PC_BAUD);
jah128 1:a6728adaf7e7 95 pc.attach(this,&Robot::_pc_rx_callback, Serial::RxIrq);
jah128 1:a6728adaf7e7 96 }
jah128 1:a6728adaf7e7 97 if(ENABLE_BLUETOOTH) {
jah128 1:a6728adaf7e7 98 bt.baud(BLUETOOTH_BAUD);
jah128 1:a6728adaf7e7 99 bt.attach(this,&Robot::_bt_rx_callback, Serial::RxIrq);
jah128 1:a6728adaf7e7 100 }
jah128 1:a6728adaf7e7 101 }
jah128 1:a6728adaf7e7 102
jah128 0:8a2dd255c508 103 // Private functions
jah128 0:8a2dd255c508 104
jah128 1:a6728adaf7e7 105 void Robot::_pc_rx_callback()
jah128 1:a6728adaf7e7 106 {
jah128 1:a6728adaf7e7 107
jah128 1:a6728adaf7e7 108 }
jah128 1:a6728adaf7e7 109
jah128 1:a6728adaf7e7 110 void Robot::_bt_rx_callback()
jah128 1:a6728adaf7e7 111 {
jah128 1:a6728adaf7e7 112
jah128 1:a6728adaf7e7 113 }
jah128 1:a6728adaf7e7 114
jah128 1:a6728adaf7e7 115
jah128 0:8a2dd255c508 116 void Robot::_update_minutes()
jah128 0:8a2dd255c508 117 {
jah128 0:8a2dd255c508 118 mbed_uptime.reset();
jah128 0:8a2dd255c508 119 timer_minute_count += 5;
jah128 0:8a2dd255c508 120 }