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

Prettier way of driving the control loop

Browse Source
This commit is contained in:
philsson 2018-08-31 18:11:32 +02:00 committed by Philip Johansson
parent cbe9ae46d4
commit 806a80ebe0
2 changed files with 21 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@ -3,6 +3,7 @@
//"C_Cpp.intelliSenseEngine": "Default" // Does not work with "Go to definition" etc. //"C_Cpp.intelliSenseEngine": "Default" // Does not work with "Go to definition" etc.
"C_Cpp.intelliSenseEngine": "Tag Parser", "C_Cpp.intelliSenseEngine": "Tag Parser",
"files.associations": { "files.associations": {
"cmath": "cpp" "cmath": "cpp",
"queue": "cpp"
} }
} }

55
main.cpp
View File

@ -19,8 +19,6 @@ using namespace control;
EventQueue queue; EventQueue queue;
Ticker queueReader;
// 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
@ -38,8 +36,17 @@ Servo servo(PA_0);
InterruptIn gyroINT(PC_4); InterruptIn gyroINT(PC_4);
// Draft of some control function // Draft of some control function
void controlFunc(int a) // this runs in the context of eventThread and is triggered by the gyro ISR pin
void controlFunc()
{ {
static int i = 0;
i++;
if (i > 100)
{
i = 0;
ledOrg = !ledOrg;
}
static incrementalLPF filter; static incrementalLPF filter;
double accY = 1; double accY = 1;
accY = imu.read_acc(1); accY = imu.read_acc(1);
@ -66,27 +73,6 @@ void pulseLedContext()
} }
} }
void readQueue()
{
//queue.dispatch(0);
}
// Interrupt driven function
// At the moment its running at 100hz
void ISRGYRO()
{
static int i = 0;
i++;
if (i > 100)
{
i = 0;
ledOrg = !ledOrg;
}
// Report that there is new data to read
// TODO: This hack has to be done in a better way
queue.call(controlFunc, 0);
}
int i = 0; int i = 0;
// main() runs in its own thread // main() runs in its own thread
@ -98,8 +84,13 @@ int main() {
Thread ledPulseThread; Thread ledPulseThread;
ledPulseThread.start(callback(&pulseLedContext)); ledPulseThread.start(callback(&pulseLedContext));
// Register a event queue reader at set interval //
queueReader.attach(&readQueue, 0.001); Thread eventThread;
eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
// Function to attach to gyro interrupt
gyroINT.rise(queue.event(&controlFunc));
// Enable motor controllers (Will power the motors with no movement) // Enable motor controllers (Will power the motors with no movement)
motor1.enable(); motor1.enable();
@ -109,8 +100,6 @@ int main() {
// (To test that timers don't interfeer with each other) // (To test that timers don't interfeer with each other)
motor2.setSpeed(90.0); motor2.setSpeed(90.0);
// Function to attach to gyro interrupt
gyroINT.rise(&ISRGYRO);
// MPU startup // MPU startup
if(imu.init(10,BITS_DLPF_CFG_188HZ)){ if(imu.init(10,BITS_DLPF_CFG_188HZ)){
@ -132,13 +121,7 @@ int main() {
servo.sweep(1.0, 1.5, 3); servo.sweep(1.0, 1.5, 3);
servo.setPosition(-0.5); servo.setPosition(-0.2);
while (true) wait(osWaitForever);
{
// HACK!!!
// Dispatch any content of the event queue.
queue.dispatch(0);
Thread::wait(1);
}
} }