Psi Swarm Robot / PRRobot

Practical Robotics Modular Robot Library

Dependents:   ModularRobot

robot.cpp@4:c2e933d53bea, 2017-01-02 (annotated)

Committer:
jah128
Date:
Mon Jan 02 15:17:22 2017 +0000
Revision:
4:c2e933d53bea
Parent:
3:8762f6b2ea8d
Child:
5:6da8daaeb9f7
Added serialcomms library (adapted from Psi) and LED animations

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 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 4:c2e933d53bea 101 led.start_animation(9,40);
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 4:c2e933d53bea 107 led.start_animation(0,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 }