Diff: robot.cpp

Revision:

3:8762f6b2ea8d

Parent:

2:bf34b86aa0f3

Child:

4:c2e933d53bea

--- a/robot.cpp	Sat Nov 26 21:50:10 2016 +0000
+++ b/robot.cpp	Mon Nov 28 22:41:14 2016 +0000
@@ -13,18 +13,43 @@
 Motors motors;
 
 Serial pc(USBTX,USBRX);
+DigitalIn rpi1(p5,PullDown);
 I2C primary_i2c (p9, p10);
-Serial bt(p14,p13);
+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;
@@ -60,6 +85,15 @@
     //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)