135 lines
3.1 KiB
C++
135 lines
3.1 KiB
C++
#include "mbed.h"
|
|
|
|
// STD
|
|
#include <cmath>
|
|
|
|
// Mbed
|
|
#include "mbed_events.h"
|
|
|
|
// target definitions
|
|
#include "src/targets/revo_f4/pins.h"
|
|
// Mmath
|
|
#include "src/math/Utilities.h"
|
|
// Drivers
|
|
#include "src/drivers/MPU6000.h"
|
|
#include "src/drivers/stepper.h"
|
|
#include "src/drivers/servo.h"
|
|
// Control
|
|
#include "src/control/lpf.h"
|
|
#include "src/control/PID.h"
|
|
#include "src/control/ImuFusion.h"
|
|
#include "src/control/MadgwickAHRS.h"
|
|
// Serialization
|
|
#include "src/serialization/RCProtocol.h"
|
|
|
|
#define WHEEL_SIZE 0.09f
|
|
|
|
using namespace targets::revo_f4;
|
|
using namespace drivers;
|
|
using namespace control;
|
|
using namespace math;
|
|
using namespace serialization;
|
|
|
|
// Serial port (Servo Outputs)
|
|
//Serial serial(PA_2, PA_3, 57600);
|
|
RawSerial serial(PA_2, PA_3, 57600);
|
|
RCProtocol RC;
|
|
|
|
// MemoryPool<RCProtocol::Packet, 16> mpool;
|
|
// Queue<RCProtocol::Packet, 16> RCQueue;
|
|
|
|
PwmOut ledBlue(D4);
|
|
DigitalOut ledOrg(D5);
|
|
|
|
Servo servo(PA_0);
|
|
|
|
DigitalOut motorL(PC_8);
|
|
DigitalOut motorR(PC_6);
|
|
|
|
//Timer timer;
|
|
// This context just pulses the blue LED
|
|
void pulseLedContext()
|
|
{
|
|
while (true)
|
|
{
|
|
float in, out;
|
|
|
|
for (in = 0; in < 2*PI; in = in + PI/50)
|
|
{
|
|
out = sin(in) + 0.5;
|
|
// We cannot set pulsewidth or period due to
|
|
// sharing timer with motor outputs. "write"
|
|
// instead sets the dutycycle in range 0-1
|
|
ledBlue.write(out);
|
|
Thread::wait(10);
|
|
}
|
|
}
|
|
}
|
|
|
|
void serialContext(void)
|
|
{
|
|
//ledOrg = 0;
|
|
static int i = 0;
|
|
static RCProtocol::Packet pkt;
|
|
// while (true)
|
|
// {
|
|
// Receive
|
|
while (serial.readable())
|
|
{
|
|
bool newPackage = RC.appendByte(serial.getc());
|
|
|
|
if (newPackage)
|
|
{
|
|
//RCProtocol::Packet* pPacket = mpool.alloc();
|
|
pkt = RC.read();
|
|
//RCQueue.put(pPacket);
|
|
servo.setPosition((float)pkt.Throttle/1000.0f);
|
|
//ledOrg.write((int)pkt.Enabled);
|
|
|
|
i++;
|
|
i = i % 100;
|
|
if (i == 0)
|
|
{
|
|
ledOrg = !ledOrg;
|
|
}
|
|
}
|
|
// }
|
|
// Sleep some?
|
|
//Thread::wait(1);
|
|
|
|
// Transmit
|
|
// serial.putc()...
|
|
}
|
|
}
|
|
|
|
|
|
// main() runs in its own thread
|
|
int main() {
|
|
|
|
motorR.write(1);
|
|
motorL.write(1);
|
|
ledOrg.write(1);
|
|
|
|
serial.set_dma_usage_rx(DMA_USAGE_ALWAYS);
|
|
// (USART2)->CR1 &= ~USART_CR1_UE;
|
|
// wait(1);
|
|
serial.attach(&serialContext, Serial::RxIrq);
|
|
// wait(1);
|
|
// (USART2)->CR1 |= USART_CR1_UE;
|
|
|
|
// ISR won't be called if the serial is not emptied first.
|
|
serialContext();
|
|
// Serial in / out
|
|
// Thread readUartThread;
|
|
// readUartThread.start(callback(&serialContext));
|
|
//serial.set_dma_usage_rx(DMA_USAGE_ALWAYS);
|
|
//serial.attach(&serialContext);
|
|
|
|
// Start the pulsing blue led
|
|
Thread ledPulseThread;
|
|
ledPulseThread.start(callback(&pulseLedContext));
|
|
|
|
servo.setPosition(-0.2);
|
|
|
|
wait(osWaitForever);
|
|
} |