127 lines
2.7 KiB
C++
127 lines
2.7 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;
|
|
|
|
|
|
// 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
|
|
// 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;
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
|
|
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));
|
|
|
|
//
|
|
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)
|
|
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);
|
|
|
|
|
|
// 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);
|
|
|
|
Thread::wait(1000);
|
|
|
|
servo.nod();
|
|
|
|
Thread::wait(1000);
|
|
|
|
servo.sweep(1.0, 1.5, 3);
|
|
|
|
servo.setPosition(-0.2);
|
|
|
|
wait(osWaitForever);
|
|
} |