philip/balancing-robot
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

UART-1

Browse Source
This commit is contained in:
philsson 2020-06-22 17:34:56 +02:00
parent 7db32ebe18
commit f15647ffb2
6 changed files with 122 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
NodeMCU/OSCWifiBridge/OSCWifiBridge.ino
View File

@ -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()
{

38
main.cpp
View File

@ -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);

BIN
mobile/Robot.touchosc
View File

Binary file not shown.

2
src/control/PID.cpp
View File

@ -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)
{
}

22
src/serialization/RCProtocol.cpp
View File

@ -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];
}

19
src/serialization/RCProtocol.h
View File

@ -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)
{