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

Some simple serial test

Browse Source 
Just write back the serial data that comes in
This commit is contained in:
philsson 2018-09-11 16:27:49 +02:00
parent fcb2359914
commit 220de9612c
1 changed files with 27 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
main.cpp
View File

@ -28,6 +28,8 @@ using namespace math;
EventQueue queue; EventQueue queue;
// Serial port (Servo Outputs)
Serial serialMotorOutputs(PA_2, PA_3, 115200);
// MPU setup // MPU setup
SPI spi(PA_7, PA_6, PA_5); //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz SPI spi(PA_7, PA_6, PA_5); //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
@ -50,7 +52,7 @@ Timer timer;
controllerPI throttleControl(0.0025, 0.01, 5, 0); // 0.065, 0.05, 12, 40 controllerPI throttleControl(0.0025, 0.01, 5, 0); // 0.065, 0.05, 12, 40
// TODO: Figure out some good values // TODO: Figure out some good values
controllerPD angleControl(25.0, 35.25, 400); controllerPD angleControl(10.0, 146.0, 400);
// Alternatively try controllerPD2 which has another Dterm implementation // Alternatively try controllerPD2 which has another Dterm implementation
// Draft of some control function // Draft of some control function
@ -66,6 +68,7 @@ void controlFunc()
// Retrieve IMU angle (Only x-axis implemented for now) // Retrieve IMU angle (Only x-axis implemented for now)
float angleX = imuFusion.getAngle(dT); float angleX = imuFusion.getAngle(dT);
float forceX = sin(angleX/180*PI);
// Reset anything left in the IMU FIFO queue // Reset anything left in the IMU FIFO queue
imu.fifo_reset(); imu.fifo_reset();
@ -75,10 +78,14 @@ void controlFunc()
{ {
controlOutput = 0.0f; // rinse integral controlOutput = 0.0f; // rinse integral
disabled = true; disabled = true;
motorL.disable();
motorR.disable();
} }
else if (abs(angleX) < (float)50.0f && disabled) else if (abs(angleX) < (float)50.0f && disabled)
{ {
disabled = false; disabled = false;
motorL.enable();
motorR.enable();
} }
/* -------------------------- /* --------------------------
@ -112,9 +119,9 @@ void controlFunc()
// Integrating the output to obtain acceleration // Integrating the output to obtain acceleration
if (!disabled) if (!disabled)
{ {
controlOutput += angleControl.run(dT, angleX, angleSP); controlOutput += angleControl.run(dT, forceX, angleSP);
controlOutput = constrain(controlOutput, 800.0f); controlOutput = constrain(controlOutput, 3000.0f);
} }
/* -------------------------- /* --------------------------
@ -136,6 +143,18 @@ void controlFunc()
} }
} }
void readSerialContext()
{
while (true)
{
while (serialMotorOutputs.readable())
{
serialMotorOutputs.putc(serialMotorOutputs.getc());
}
}
Thread::wait(1000);
}
// This context just pulses the blue LED // This context just pulses the blue LED
void pulseLedContext() void pulseLedContext()
{ {
@ -155,11 +174,14 @@ void pulseLedContext()
} }
} }
int i = 0;
// main() runs in its own thread // main() runs in its own thread
int main() { int main() {
serialMotorOutputs.baud(115200);
// Test thread for serial
Thread serialThread;
serialThread.start(callback(&readSerialContext));
// MPU startup at 100hz // MPU startup at 100hz
if(imu.init(10,BITS_DLPF_CFG_188HZ)){ if(imu.init(10,BITS_DLPF_CFG_188HZ)){
printf("\nCouldn't initialize MPU6000 via SPI!"); printf("\nCouldn't initialize MPU6000 via SPI!");