359 lines
9.0 KiB
C++
359 lines
9.0 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"
|
|
#include "src/drivers/M25P16.h"
|
|
// Control
|
|
#include "src/control/lpf.h"
|
|
#include "src/control/PID.h"
|
|
#include "src/control/ImuFusion.h"
|
|
// Flash Memory - TODO: This component is not correctly implemented yet
|
|
#include "src/drivers/SerialFlash/MbedSPICommand.h"
|
|
#include "src/drivers/SerialFlash/SerialFlashDeviceCreator.h"
|
|
#include "src/drivers/SpiFlash25.h"
|
|
|
|
#define WHEEL_SIZE 0.09f
|
|
|
|
using namespace targets::revo_f4;
|
|
using namespace drivers;
|
|
using namespace control;
|
|
using namespace math;
|
|
|
|
EventQueue queue;
|
|
|
|
// Serial port (Servo Outputs)
|
|
Serial serialMotorOutputs(PA_2, PA_3, 115200);
|
|
|
|
// MPU setup
|
|
SPI spiImu(PA_7, PA_6, PA_5); //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
|
|
mpu6000_spi imu(spiImu,PA_4); //define the mpu6000 object
|
|
|
|
// Flash setup
|
|
//SPI spiFlash(PC_12, PC_11, PC_10, PA_15);
|
|
//SPI spiFlash(PC_12, PC_11, PC_10, PB_3);
|
|
//M25P16 memory(spiFlash, PB_3);
|
|
//DigitalOut radioCS(PA_15);
|
|
|
|
PwmOut ledBlue(D4);
|
|
DigitalOut ledOrg(D5);
|
|
|
|
Stepper motorL(PC_9, PC_7, PC_8);
|
|
Stepper motorR(PB_15, PB_14, PC_6);
|
|
|
|
Servo servo(PA_0);
|
|
|
|
// Interrupt pin from Gyro to MCU
|
|
InterruptIn gyroINT(PC_4);
|
|
|
|
Timer timer;
|
|
|
|
// Throttle PI Controller with some acceptable values
|
|
controllerPI throttleControl(0.0025, 0.01, 5, 0); // 0.065, 0.05, 12, 40
|
|
|
|
// TODO: Figure out some good values
|
|
controllerPD angleControl(10.0, 146.0, 400);
|
|
// Alternatively try controllerPD2 which has another Dterm implementation
|
|
|
|
// Draft of some control function
|
|
// this runs in the context of eventThread and is triggered by the gyro ISR pin
|
|
void controlFunc()
|
|
{
|
|
static ImuFusion imuFusion(&imu);
|
|
static float controlOutput(0.0f);
|
|
|
|
// Calculate dT in sec
|
|
float dT = timer.read_us()/(float)1000000.0;
|
|
timer.reset();
|
|
|
|
// Retrieve IMU angle (Only x-axis implemented for now)
|
|
float angleX = imuFusion.getAngle(dT);
|
|
float forceX = sin(angleX/180*PI);
|
|
// Reset anything left in the IMU FIFO queue
|
|
imu.fifo_reset();
|
|
|
|
// If the robot is above this angle we turn off motors
|
|
static bool disabled = false;
|
|
if (abs(angleX) > (float)50.0f && !disabled)
|
|
{
|
|
controlOutput = 0.0f; // rinse integral
|
|
disabled = true;
|
|
motorL.disable();
|
|
motorR.disable();
|
|
}
|
|
else if (abs(angleX) < (float)50.0f && disabled)
|
|
{
|
|
disabled = false;
|
|
motorL.enable();
|
|
motorR.enable();
|
|
}
|
|
|
|
/* --------------------------
|
|
Calculate estimated groundspeed
|
|
of the robot.
|
|
TODO: Inject correct input (compensate for robot rotation)
|
|
-------------------------- */
|
|
// Calculate filtered groundspeed of the robot (From deg/s to m/s)
|
|
float estimatedSpeed((motorL.getSpeed() + motorR.getSpeed())/2);
|
|
static incrementalLPF speedFilter;
|
|
float filteredEstSpeed(speedFilter.filter(estimatedSpeed));
|
|
float speedScale((1.0f/360.0f)*WHEEL_SIZE*PI);
|
|
float groundSpeed(filteredEstSpeed*speedScale);
|
|
|
|
/* --------------------------
|
|
Calculate the setpoint for
|
|
the main control (control the angle)
|
|
through this throttle control loop (PI)
|
|
TODO: TEMP turned off the throttle
|
|
control to tune the PD angle control
|
|
-------------------------- */
|
|
float throttle(0.0f); // TODO: This will be the input from remote
|
|
float angleSP = 0.0f; // Temporarily emulate an output
|
|
//float angleSP = throttleControl.run(dT, groundSpeed, throttle);
|
|
|
|
|
|
/* --------------------------
|
|
The last control loop. Angle (PD)
|
|
TODO:
|
|
-------------------------- */
|
|
// Integrating the output to obtain acceleration
|
|
if (!disabled)
|
|
{
|
|
controlOutput += angleControl.run(dT, forceX, angleSP);
|
|
|
|
controlOutput = constrain(controlOutput, 3000.0f);
|
|
}
|
|
|
|
/* --------------------------
|
|
Lastly the steering is added
|
|
straight on the output
|
|
TODO: Activate when implemented
|
|
-------------------------- */
|
|
float steering(0.0f); // This will come from remote
|
|
motorL.setSpeed(controlOutput - steering);
|
|
motorR.setSpeed(controlOutput + steering);
|
|
|
|
// Blink LED at 1hz
|
|
static int i = 0;
|
|
i++;
|
|
if (i > 100)
|
|
{
|
|
i = 0;
|
|
ledOrg = !ledOrg;
|
|
}
|
|
}
|
|
|
|
void readSerialContext()
|
|
{
|
|
while (true)
|
|
{
|
|
while (serialMotorOutputs.readable())
|
|
{
|
|
serialMotorOutputs.putc(serialMotorOutputs.getc());
|
|
}
|
|
}
|
|
Thread::wait(1000);
|
|
}
|
|
|
|
// 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 testFlash()
|
|
{
|
|
ISPICommand* pSPICommand = new MbedSPICommand(PC_12, PC_11, PC_10, PB_3); // MOSI, MISO, SCK, CS
|
|
ISerialFlashDevice* pDevice = SerialFlashDeviceCreator::Create(pSPICommand);
|
|
|
|
if (pDevice == NULL)
|
|
{
|
|
printf("Unsupported device");
|
|
return;
|
|
}
|
|
|
|
// std::string deviceName = pDevice->GetDeviceName();
|
|
// printf("%s\n", deviceName);
|
|
int capacity = pDevice->GetCapacity() / 1024;
|
|
printf("%dK x 8\n", capacity);
|
|
|
|
// int *a[3];
|
|
// *a[0] = 1;
|
|
// *a[1] = 2;
|
|
// *a[2] = 3;
|
|
|
|
// int *b[3];
|
|
|
|
int result;
|
|
|
|
// result = pDevice->Write(100, a, sizeof(a));
|
|
|
|
// result = pDevice->Read(100, b, sizeof(b));
|
|
|
|
uint8_t c(-1), d(0);
|
|
|
|
result = pDevice->Read(200, &c, sizeof(c));
|
|
|
|
wait(1);
|
|
|
|
c += 4;
|
|
|
|
result = pDevice->Write(200, &c, sizeof(c));
|
|
|
|
wait(1);
|
|
|
|
result = pDevice->Read(200, &d, sizeof(d));
|
|
|
|
}
|
|
|
|
void testFlash2()
|
|
{
|
|
SpiFlash25 flash(PC_12, PC_11, PC_10, PB_3);
|
|
|
|
char* k = flash.read_id();
|
|
|
|
char f = flash.read_status();
|
|
|
|
char a[] = "Hej och ha";
|
|
|
|
bool result;
|
|
|
|
result = flash.write(0, strlen(a), a);
|
|
|
|
char b[20];
|
|
|
|
result = flash.read(0, strlen(a), b);
|
|
|
|
printf("result");
|
|
|
|
|
|
}
|
|
|
|
// main() runs in its own thread
|
|
int main() {
|
|
|
|
serialMotorOutputs.baud(115200);
|
|
// Test thread for serial
|
|
Thread serialThread;
|
|
serialThread.start(callback(&readSerialContext));
|
|
|
|
// Init memory
|
|
DigitalOut radioCS(PA_15);
|
|
radioCS = 1; // Disable radio chip. May not be necessary
|
|
//SPI spiFlash(PC_12, PC_11, PC_10);
|
|
|
|
testFlash2();
|
|
|
|
// M25P16 memory(spiFlash, PB_3);
|
|
// bool memoryUp = memory.init();
|
|
|
|
//printf("Initialization of onboard memory " + memoryUp ? "succeded" : "failed");
|
|
|
|
//memory.chipErase();
|
|
int start = 0;
|
|
|
|
int values[3];
|
|
//memory.read(start, &values, sizeof(values));
|
|
|
|
|
|
values[0]++;
|
|
//values[1]++;
|
|
values[2]++;
|
|
|
|
//memory.write(start, values, sizeof(values));
|
|
|
|
|
|
// int values2[3];
|
|
// memory.read(start, values2, sizeof(values2));
|
|
|
|
// memory.read(0, values2, sizeof(values2));
|
|
|
|
// values[0]++;
|
|
// values[1]++;
|
|
// values[2]++;
|
|
|
|
// start += sizeof(values);
|
|
// memory.write(0, values, sizeof(values));
|
|
|
|
// memory.read(0, values2, sizeof(values2));
|
|
|
|
|
|
|
|
|
|
// MPU startup at 100hz
|
|
if(imu.init(10,BITS_DLPF_CFG_188HZ)){
|
|
printf("\nCouldn't initialize MPU6000 via SPI!");
|
|
}
|
|
|
|
wait(0.1);
|
|
// Set IMU scale
|
|
int accScale = imu.set_acc_scale(BITS_FS_16G);
|
|
wait(0.1);
|
|
int gyroScale = imu.set_gyro_scale(BITS_FS_2000DPS);
|
|
wait(0.1);
|
|
|
|
// Calibrate and Trim acc & gyro
|
|
bool calibrationResult = imu.resetOffset_gyro();
|
|
imu.calib_acc(0);
|
|
calibrationResult = imu.resetOffset_acc();
|
|
|
|
// Enable/Activate the Gyro interrupt
|
|
imu.enableInterrupt();
|
|
|
|
// Start the timer used by the control loop
|
|
timer.start(); // Used to calc dT
|
|
|
|
/*-------------- Visible start sequence ------------*/
|
|
// Start sweeping the arm
|
|
servo.sweep(0.0, 1, 2);
|
|
|
|
// Start the pulsing blue led
|
|
Thread ledPulseThread;
|
|
ledPulseThread.start(callback(&pulseLedContext));
|
|
|
|
// Create realtime eventhandler for control loop
|
|
Thread eventThread(osPriorityRealtime);
|
|
eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
|
|
|
|
// Attach gyro interrupt to add a control event
|
|
gyroINT.rise(queue.event(&controlFunc));
|
|
|
|
// Enable motor controllers (Will power the motors with no movement)
|
|
motorL.enable();
|
|
motorR.enable();
|
|
motorL.setDirection(1);
|
|
motorR.setDirection(-1);
|
|
|
|
Thread::wait(1000);
|
|
|
|
// Servo Nod to tell us that we are done
|
|
servo.nod();
|
|
|
|
servo.setPosition(-0.2);
|
|
|
|
wait(osWaitForever);
|
|
} |