diff --git a/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino b/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino index d8540d1..4e29c63 100644 --- a/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino +++ b/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino @@ -35,13 +35,18 @@ typedef struct PacketOut { int16_t Throttle; int16_t Steering; + + int16_t SpeedKp; + int16_t SpeedKi; - int16_t Kp; - int16_t Ki; - 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->Ki = 4; - pPacketOut->Kd = 5; + pPacketOut->SpeedKp = 3; + pPacketOut->SpeedKi = 4; + 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/Kd",funcKd); + + msgIN.route("/Gain/Speed/Kp",funcSpeedKp); + 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() { diff --git a/main.cpp b/main.cpp index 157c14f..48f2096 100644 --- a/main.cpp +++ b/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 = constrain(controlOutput, 3000.0f); - } + controlOutput = angleControl.run(dT, angleX, angleSP); /* -------------------------- 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); diff --git a/mobile/Robot.touchosc b/mobile/Robot.touchosc index 9caf678..0f9c677 100644 Binary files a/mobile/Robot.touchosc and b/mobile/Robot.touchosc differ diff --git a/src/control/PID.cpp b/src/control/PID.cpp index 2ef540d..ec6b0a0 100644 --- a/src/control/PID.cpp +++ b/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) { } diff --git a/src/serialization/RCProtocol.cpp b/src/serialization/RCProtocol.cpp index 95a1cb6..8442566 100644 --- a/src/serialization/RCProtocol.cpp +++ b/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]; } diff --git a/src/serialization/RCProtocol.h b/src/serialization/RCProtocol.h index 3fca330..847671a 100644 --- a/src/serialization/RCProtocol.h +++ b/src/serialization/RCProtocol.h @@ -16,20 +16,27 @@ public: int16_t Throttle; int16_t Steering; - int16_t Kp; - int16_t Ki; - int16_t Kd; + int16_t SpeedKp; + int16_t SpeedKi; + + int16_t AngleKp; + int16_t AngleKi; + int16_t AngleKd; bool Enabled; + bool SpeedControl; + uint8_t Checksum; PacketIn() : Throttle(0) , Steering(0) - , Kp(0) - , Ki(0) - , Kd(0) + , SpeedKp(0) + , SpeedKi(0) + , AngleKp(0) + , AngleKi(0) + , AngleKd(0) , Enabled(false) , Checksum(0) {