Psi Swarm Robot / PRRobot

Practical Robotics Modular Robot Library

Dependents:   ModularRobot

robot.cpp@3:8762f6b2ea8d, 2016-11-28 (annotated)

Committer:
jah128
Date:
Mon Nov 28 22:41:14 2016 +0000
Revision:
3:8762f6b2ea8d
Parent:
2:bf34b86aa0f3
Child:
4:c2e933d53bea
Added first RPi input

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