GDP 4 / Mbed 2 deprecated pid-car-example

car using PID from centre line

Dependencies:   FRDM-TFC mbed CBuffer XBEE mbed_angular_speed motor2 MMA8451Q

Fork of KL25Z_Camera_Test by GDP 4

Diff: main.cpp

Revision:
41:d74878640739
Parent:
40:10e8e80af7da
Child:
42:4395ede5781e
Child:
43:649473c5a12b
--- a/main.cpp	Sat Jan 21 11:59:20 2017 +0000
+++ b/main.cpp	Wed Jan 25 15:46:50 2017 +0000
@@ -504,6 +504,14 @@
 
 }
 
+float readFloat() {
+    byte_float_union.bytes[0] = xb.cBuffer->read();
+    byte_float_union.bytes[1] = xb.cBuffer->read();
+    byte_float_union.bytes[2] = xb.cBuffer->read();
+    byte_float_union.bytes[3] = xb.cBuffer->read();
+    return byte_float_union._float;    
+}
+
 // Listen for incoming commands from telemetry program and change car variables
 inline void handleComms() {
     if(curr_cmd != 0) {
@@ -511,24 +519,10 @@
                 // Change the PID values of the servo controller
                 case CHANGE_PID:
                     if(xb.cBuffer->available() >= 12) {
-                    
-                        byte_float_union.bytes[0] = xb.cBuffer->read();
-                        byte_float_union.bytes[1] = xb.cBuffer->read();
-                        byte_float_union.bytes[2] = xb.cBuffer->read();
-                        byte_float_union.bytes[3] = xb.cBuffer->read();
-                        servo_pid.Kp = byte_float_union._float; 
                         
-                        byte_float_union.bytes[0] = xb.cBuffer->read();
-                        byte_float_union.bytes[1] = xb.cBuffer->read();
-                        byte_float_union.bytes[2] = xb.cBuffer->read();
-                        byte_float_union.bytes[3] = xb.cBuffer->read();
-                        servo_pid.Ki = byte_float_union._float; 
-                        
-                        byte_float_union.bytes[0] = xb.cBuffer->read();
-                        byte_float_union.bytes[1] = xb.cBuffer->read();
-                        byte_float_union.bytes[2] = xb.cBuffer->read();
-                        byte_float_union.bytes[3] = xb.cBuffer->read();
-                        servo_pid.Kd = byte_float_union._float; 
+                        servo_pid.Kp = readFloat();
+                        servo_pid.Ki = readFloat();
+                        servo_pid.Kd = readFloat();
                         
                         sendString("pid= Kp: %f, Ki: %f, Kd: %f",  servo_pid.Kp, servo_pid.Ki, servo_pid.Kd);
                         
@@ -551,11 +545,7 @@
                 // Change the electronic differential coefficient
                 case CHANGE_ED:
                  if(xb.cBuffer->available() >= 4) {
-                        byte_float_union.bytes[0] = xb.cBuffer->read();
-                        byte_float_union.bytes[1] = xb.cBuffer->read();
-                        byte_float_union.bytes[2] = xb.cBuffer->read();
-                        byte_float_union.bytes[3] = xb.cBuffer->read();
-                        ed_tune = byte_float_union._float; 
+                        ed_tune = readFloat();
                         curr_cmd = 0;
                     }
                 break;