149 lines
3.2 KiB
C++
149 lines
3.2 KiB
C++
#include "mbed.h"
|
|
|
|
#include <cmath>
|
|
|
|
#include "mbed_events.h"
|
|
|
|
#include "src/targets/revo_f4/pins.h"
|
|
#include "src/drivers/MPU6000.h"
|
|
#include "src/drivers/stepper.h"
|
|
#include "src/drivers/servo.h"
|
|
#include "src/control/lpf.h"
|
|
|
|
#ifndef PI
|
|
#define PI 3.14159265358979f
|
|
#endif
|
|
|
|
using namespace drivers;
|
|
using namespace control;
|
|
|
|
EventQueue queue;
|
|
|
|
Ticker queueReader;
|
|
|
|
|
|
// MPU setup
|
|
SPI spi(PA_7, PA_6, PA_5); //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
|
|
mpu6000_spi imu(spi,PA_4); //define the mpu6000 object
|
|
|
|
PwmOut led1(D4); //PB_5
|
|
DigitalOut ledOrg(D5);
|
|
|
|
Stepper motor1(PC_9, PC_7, PC_8);
|
|
Stepper motor2(PB_15, PB_14, PC_6);
|
|
|
|
Servo servo(PA_0);
|
|
|
|
// Interrupt pin from Gyro to MCU
|
|
InterruptIn gyroINT(PC_4);
|
|
|
|
// Draft of some control function
|
|
void controlFunc(int a)
|
|
{
|
|
static incrementalLPF filter;
|
|
double accY = 1;
|
|
accY = imu.read_acc(1);
|
|
double filtered = filter.filter(accY);
|
|
motor1.setSpeed(360.0*2*filtered);
|
|
}
|
|
|
|
// 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
|
|
led1.write(out);
|
|
Thread::wait(10);
|
|
}
|
|
}
|
|
}
|
|
|
|
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;
|
|
|
|
// main() runs in its own thread
|
|
int main() {
|
|
|
|
// Start sweeping the arm
|
|
servo.sweep(0.0, 1, 2);
|
|
|
|
Thread ledPulseThread;
|
|
ledPulseThread.start(callback(&pulseLedContext));
|
|
|
|
// Register a event queue reader at set interval
|
|
queueReader.attach(&readQueue, 0.001);
|
|
|
|
// Enable motor controllers (Will power the motors with no movement)
|
|
motor1.enable();
|
|
motor2.enable();
|
|
|
|
// Set motor 2 to a fixed speed
|
|
// (To test that timers don't interfeer with each other)
|
|
motor2.setSpeed(90.0);
|
|
|
|
// Function to attach to gyro interrupt
|
|
gyroINT.rise(&ISRGYRO);
|
|
|
|
// MPU startup
|
|
if(imu.init(10,BITS_DLPF_CFG_188HZ)){
|
|
printf("\nCouldn't initialize MPU6000 via SPI!");
|
|
}
|
|
|
|
// Enable/Activate the Gyro interrupt
|
|
imu.enableInterrupt();
|
|
|
|
// Read some test values
|
|
int wmi = imu.whoami();
|
|
int scale = imu.set_acc_scale(BITS_FS_16G);
|
|
|
|
// Stop the sweeping arm and do some other stuff with it
|
|
servo.stop();
|
|
|
|
Thread::wait(1000);
|
|
|
|
servo.nod();
|
|
|
|
Thread::wait(1000);
|
|
|
|
servo.sweep(1.0, 1.5, 3);
|
|
|
|
servo.stop();
|
|
|
|
servo.setPosition(-1);
|
|
|
|
while (true)
|
|
{
|
|
// HACK!!!
|
|
// Dispatch any content of the event queue.
|
|
queue.dispatch(0);
|
|
Thread::wait(1);
|
|
}
|
|
} |