UART-1

NodeMCU/OSCWifiBridge/OSCWifiBridge.ino

@@ -36,12 +36,17 @@ typedef struct PacketOut {
   int16_t Throttle;
   int16_t Steering;
 
-  int16_t Kp;
+  int16_t SpeedKp;
-  int16_t Ki;
+  int16_t SpeedKi;
-  int16_t Kd;
+  
+  int16_t AngleKp;
+  int16_t AngleKi;
+  int16_t AngleKd;
   
   bool Enabled;
 
+  bool SpeedControl;
+
   uint8_t checksum;
   
 } __attribute__ ((__packed__));
@@ -97,10 +102,13 @@ void setup() {
 
   pPacketOut->Throttle = 1;
   pPacketOut->Steering = 2;
-  pPacketOut->Kp = 3;
+  pPacketOut->SpeedKp = 3;
-  pPacketOut->Ki = 4;
+  pPacketOut->SpeedKi = 4;
-  pPacketOut->Kd = 5;
+  pPacketOut->AngleKp = 3;
+  pPacketOut->AngleKi = 4;
+  pPacketOut->AngleKd = 5;
   pPacketOut->Enabled = false;
+  pPacketOut->SpeedControl = true;
   pPacketOut->checksum = 0;
 }
 
@@ -125,9 +133,15 @@ void OSCMsgReceive()
       msgIN.route("/Robot/Enable",funcEnabled);
       msgIN.route("/Fader/Throttle",funcThrottle);
       msgIN.route("/Fader/Steering",funcSteering);
-      msgIN.route("/Gain/Kp",funcKp);
+      
-      msgIN.route("/Gain/Ki",funcKi);
+      msgIN.route("/Gain/Speed/Kp",funcSpeedKp);
-      msgIN.route("/Gain/Kd",funcKd);
+      msgIN.route("/Gain/Speed/Ki",funcSpeedKi);
+      
+      msgIN.route("/Gain/Angle/Kp",funcAngleKp);
+      msgIN.route("/Gain/Angle/Ki",funcAngleKi);
+      msgIN.route("/Gain/Angle/Kd",funcAngleKd);
+
+      msgIN.route("/SpeedControl",funcSpeedControl);
     }
   }
 }
@@ -189,48 +203,85 @@ void funcSteering(OSCMessage &msg, int addrOffset ){
   pPacketOut->Steering = value;
 }
 
-void funcKp(OSCMessage &msg, int addrOffset ){
+void funcSpeedKp(OSCMessage &msg, int addrOffset ){
 
   int value = msg.getFloat(0);
-  OSCMessage msgOUT("/Gain/Kp");
+  OSCMessage msgOUT("/Gain/Speed/Kp");
   msgOUT.add(value);
   sendUdp(&msgOUT);
-  pPacketOut->Kp = value;
+  pPacketOut->SpeedKp = value;
 
   // Redo this for label
-  OSCMessage msgLABEL("/Gain/KpOut");
+  OSCMessage msgLABEL("/Gain/Speed/KpOut");
   msgLABEL.add(value);
   sendUdp(&msgLABEL);
 }
 
-void funcKi(OSCMessage &msg, int addrOffset ){
+void funcSpeedKi(OSCMessage &msg, int addrOffset ){
 
   int value = msg.getFloat(0);
-  OSCMessage msgOUT("/Gain/Ki");
+  OSCMessage msgOUT("/Gain/Speed/Ki");
   msgOUT.add(value);
   sendUdp(&msgOUT);
-  pPacketOut->Ki = value;
+  pPacketOut->SpeedKi = value;
 
   // Redo this for label
-  OSCMessage msgLABEL("/Gain/KiOut");
+  OSCMessage msgLABEL("/Gain/Speed/KiOut");
   msgLABEL.add(value);
   sendUdp(&msgLABEL);
 }
 
-void funcKd(OSCMessage &msg, int addrOffset ){
+void funcAngleKp(OSCMessage &msg, int addrOffset ){
 
   int value = msg.getFloat(0);
-  OSCMessage msgOUT("/Gain/Kd");
+  OSCMessage msgOUT("/Gain/Angle/Kp");
   msgOUT.add(value);
   sendUdp(&msgOUT);
-  pPacketOut->Kd = value;
+  pPacketOut->AngleKp = value;
 
   // Redo this for label
-  OSCMessage msgLABEL("/Gain/KdOut");
+  OSCMessage msgLABEL("/Gain/Angle/KpOut");
   msgLABEL.add(value);
   sendUdp(&msgLABEL);
 }
 
+void funcAngleKi(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Gain/Angle/Ki");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacketOut->AngleKi = value;
+
+  // Redo this for label
+  OSCMessage msgLABEL("/Gain/Angle/KiOut");
+  msgLABEL.add(value);
+  sendUdp(&msgLABEL);
+}
+
+void funcAngleKd(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Gain/Angle/Kd");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacketOut->AngleKd = value;
+
+  // Redo this for label
+  OSCMessage msgLABEL("/Gain/Angle/KdOut");
+  msgLABEL.add(value);
+  sendUdp(&msgLABEL);
+}
+
+void funcSpeedControl(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/SpeedControl");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacketOut->SpeedControl = value;
+}
+
 // Send OSC packets updating touchOSC on the mobile
 void remotePresentation()
 {

main.cpp

@@ -48,7 +48,6 @@ mpu6000_spi imu(spi,PA_4);   //define the mpu6000 object
 PwmOut ledBlue(D4);
 DigitalOut ledOrg(D5);
 
-// Stepper motorL(PC_9, PB_1, PC_8);
 Stepper motorL(PC_9, PC_7, PC_8);
 Stepper motorR(PB_15, PB_14, PC_6);
 
@@ -64,10 +63,10 @@ Madgwick madgwick;
 Timer timer;
 
 // A PI controller for throttle control
-controllerPI throttleControl(35.0, 3.0, 20, 60); // 35.0, 3.0, 20, 60
+controllerPI throttleControl(30.0f, 3.0f, 20.0f/*S*/, 60.0f/*SI*/); // 35.0, 3.0, 20, 60
 
 // A PID for angle control
-controllerPID angleControl(100.0f, 200.0f, 0.5f, 4000, 2000); // 100.0f, 200.0f, 0.5f, 4000, 2000
+controllerPID angleControl(100.0f, 200.0f, 0.05f, 4000.0f/*S*/, 2000.0f/*SI*/); // 100.0f, 200.0f, 0.5f, 4000, 2000
 
 // The control function to balance the robot
 // Ran at high prio and triggered as an event by the gyro Interrupt pin
@@ -83,11 +82,12 @@ void runControl()
         remote = *pPacketIn;
         mpool.free(pPacketIn);
 
-        throttleControl.setGainScaling(remote.Ki, remote.Kd);
+        throttleControl.setGainScaling(remote.SpeedKp, remote.SpeedKi);
         //throttleControl.setGainScaling(0, 0);
 
         //angleControl.setGainScaling(remote.Kp, remote.Ki, remote.Kd);
-        angleControl.setGainScaling(remote.Kp, 0, -900);
+        //angleControl.setGainScaling(remote.AngleKp, 0, -900);
+        angleControl.setGainScaling(remote.AngleKp, remote.AngleKi, remote.AngleKd);
 
         servo.setPosition((float)remote.Throttle/1000.0f);
     }
@@ -111,7 +111,7 @@ void runControl()
 
     // If the robot is above this angle we turn off motors
     static bool disabled = false;
-    if (!remote.Enabled || (abs(angleX) > (float)50.0f && !disabled))
+    if (!remote.Enabled || (abs(angleX) > (float)50.0f && !disabled) || lipo.getState() == Lipo::Empty)
     {
         controlOutput = 0.0f; // rinse integral
         disabled = true;
@@ -166,30 +166,34 @@ void runControl()
     the main control (control the angle)
     through this throttle control loop (PI)
     -------------------------- */
-    float throttle = remote.Throttle*0.001f;
+    float throttle = remote.Throttle*0.0015f;
 
-    if (remote.Throttle < 50 && remote.Throttle > -50) {throttle = 0;}
+    if (remote.Throttle < 100 && remote.Throttle > -100) {throttle = 0;}
     
     // TODO: Figure out why we have the wrong sign here. I also had to reverse motors
     // For the angle control loop. Is the madgwick filter outputting the wrong sign?
-    float angleSP = -throttleControl.run(dT, estimatedGroundSpeed, throttle);
+    float angleSP(0);
+    if (remote.SpeedControl)
+    {
+        angleSP = -throttleControl.run(dT, estimatedGroundSpeed, throttle);
+    }
+    else
+    {
+        angleSP = -remote.Throttle*0.015;
+    }
+    
 
     /* --------------------------
     The last control loop. Angle (PD)
     -------------------------- */
-    if (!disabled)
+    controlOutput = angleControl.run(dT, angleX, angleSP);
-    {
-        controlOutput = angleControl.run(dT, angleX, angleSP);
-
-        controlOutput = constrain(controlOutput, 3000.0f);
-    }
 
     /* --------------------------
      Lastly the steering is added
      straight on the output.
     -------------------------- */
     float steering = remote.Steering*0.5;
-    if (remote.Steering < 50 && remote.Steering > -50) {steering = 0;}
+    if (remote.Steering < 100 && remote.Steering > -100) {steering = 0;}
     motorL.setSpeed(controlOutput - steering);
     motorR.setSpeed(controlOutput + steering);
 
@@ -236,9 +240,7 @@ void serialWrite()
         // writing a packet we sleep
         if (done)
         {
-            float voltage = lipo.getVoltage();
             float charge = lipo.getCharge();
-            Lipo::State state = lipo.getState();
 
             packetOut.BatteryLevel = charge*100;
             RC.setOutput(packetOut);

src/control/PID.cpp

@@ -126,7 +126,7 @@ controllerPID::controllerPID(float kP, float kI, float kD, float saturation, flo
 , m_lastError(0.0f)
 , m_iTermSaturation(iTermSaturation)
 , m_integrator(0.0f)
-, m_pt1FilterApply4(20.0f)
+, m_pt1FilterApply4(5.0f)
 {
 }
 

src/serialization/RCProtocol.cpp

@@ -5,11 +5,11 @@ namespace serialization {
 namespace {
 
 //! Example package
-void testParser()
+RCProtocol::PacketIn testParser()
 {
     static RCProtocol RCProt;
 
-    uint8_t dummy[16];
+    uint8_t dummy[sizeof(RCProtocol::PacketIn)];
 
     // Magic word 233573869 / 0xDEC0DED
     dummy[0] = 0xED; // 11101101
@@ -26,15 +26,23 @@ void testParser()
     dummy[6] = 0xFF;
     dummy[7] = 0xFF;
 
-    // Kp 1234 
+    // Speed Kp 1234 
     dummy[8] = 0xD2;
     dummy[9] = 0x04;
 
-    // Ki -1234 
+    // Speed Ki -1234 
     dummy[10] = 0x2E;
     dummy[11] = 0xFB;
 
-    // Kd 10000 
+    // Angle Kp 1234 
+    dummy[8] = 0xD2;
+    dummy[9] = 0x04;
+
+    // Angle Ki -1234 
+    dummy[10] = 0x2E;
+    dummy[11] = 0xFB;
+
+    // Angle Kd 10000 
     dummy[12] = 0x10;
     dummy[13] = 0x27;
 
@@ -51,8 +59,10 @@ void testParser()
         if (newPackage)
         {
             RCProtocol::PacketIn prot = RCProt.readPacket();
+            return prot;
         }
     }
+    return RCProtocol::PacketIn();
 }
 
 }
@@ -140,7 +150,7 @@ void RCProtocol::setOutput(RCProtocol::PacketOut& packetOut)
 
     uint16_t sum = 0;
 
-    for (int i = 4; i < sizeof(RCProtocol::PacketOut) - 1; i++)
+    for (uint i = 4; i < sizeof(RCProtocol::PacketOut) - 1; i++)
     {
         sum += pByte[i];
     }

src/serialization/RCProtocol.h

@@ -16,20 +16,27 @@ public:
         int16_t Throttle;
         int16_t Steering;
 
-        int16_t Kp;
+        int16_t SpeedKp;
-        int16_t Ki;
+        int16_t SpeedKi;
-        int16_t Kd;
+
+        int16_t AngleKp;
+        int16_t AngleKi;
+        int16_t AngleKd;
 
         bool Enabled;
 
+        bool SpeedControl;
+
         uint8_t Checksum;
         
         PacketIn()
         : Throttle(0)
         , Steering(0)
-        , Kp(0)
+        , SpeedKp(0)
-        , Ki(0)
+        , SpeedKi(0)
-        , Kd(0)
+        , AngleKp(0)
+        , AngleKi(0)
+        , AngleKd(0)
         , Enabled(false)
         , Checksum(0)
         {