unkown work UART 2
This commit is contained in:
parent
aafc43e230
commit
e0648f23ab
@ -11,7 +11,7 @@
|
||||
char ssid[] = "ROBOT";
|
||||
char pass[] = "myrobotisawesome";
|
||||
|
||||
const long sendInterval = 25; // in ms
|
||||
const long sendInterval = 100; // in ms
|
||||
|
||||
WiFiUDP Udp;
|
||||
|
||||
@ -82,11 +82,11 @@ void setup() {
|
||||
pPacket->MagicWordLow = 0x0DED;
|
||||
pPacket->MagicWordHigh = 0x0DEC;
|
||||
|
||||
pPacket->Throttle = 0;
|
||||
pPacket->Steering = 0;
|
||||
pPacket->Kp = 0;
|
||||
pPacket->Ki = 0;
|
||||
pPacket->Kd = 0;
|
||||
pPacket->Throttle = 1;
|
||||
pPacket->Steering = 2;
|
||||
pPacket->Kp = 3;
|
||||
pPacket->Ki = 4;
|
||||
pPacket->Kd = 5;
|
||||
pPacket->Enabled = false;
|
||||
pPacket->checksum = 0;
|
||||
}
|
||||
@ -224,7 +224,7 @@ void loop() {
|
||||
{
|
||||
// If something can be printed there is enough time
|
||||
// between iterations
|
||||
//Serial.println("Sending data");
|
||||
Serial.println("Sending data");
|
||||
previousMillis = currentMillis;
|
||||
sendPacket(); // Send on UART
|
||||
}
|
||||
|
||||
242
main.cpp
242
main.cpp
@ -18,6 +18,7 @@
|
||||
#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"
|
||||
|
||||
@ -29,147 +30,23 @@ using namespace control;
|
||||
using namespace math;
|
||||
using namespace serialization;
|
||||
|
||||
EventQueue queue;
|
||||
|
||||
// Serial port (Servo Outputs)
|
||||
Serial serial(PA_2, PA_3, 57600);
|
||||
//Serial serial(PA_2, PA_3, 57600);
|
||||
RawSerial serial(PA_2, PA_3, 57600);
|
||||
RCProtocol RC;
|
||||
|
||||
// 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
|
||||
// MemoryPool<RCProtocol::Packet, 16> mpool;
|
||||
// Queue<RCProtocol::Packet, 16> RCQueue;
|
||||
|
||||
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;
|
||||
|
||||
// TODO: Figure out some good 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 serialContext()
|
||||
{
|
||||
static RCProtocol RC;
|
||||
|
||||
while (true)
|
||||
{
|
||||
// Receive
|
||||
while (serial.readable())
|
||||
{
|
||||
bool newPackage = RC.appendByte(serial.getc());
|
||||
|
||||
if (newPackage)
|
||||
{
|
||||
RCProtocol::Packet packet = RC.read();
|
||||
}
|
||||
}
|
||||
// Sleep some?
|
||||
|
||||
// Transmit
|
||||
// serial.putc()...
|
||||
}
|
||||
}
|
||||
|
||||
DigitalOut motorL(PC_8);
|
||||
DigitalOut motorR(PC_6);
|
||||
|
||||
//Timer timer;
|
||||
// This context just pulses the blue LED
|
||||
void pulseLedContext()
|
||||
{
|
||||
@ -189,62 +66,69 @@ void pulseLedContext()
|
||||
}
|
||||
}
|
||||
|
||||
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));
|
||||
|
||||
// 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);
|
||||
// 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));
|
||||
|
||||
// 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);
|
||||
|
||||
@ -12,7 +12,7 @@ Stepper::Stepper(PinName stepPin,
|
||||
, m_dir(dirPin)
|
||||
, m_en(enPin)
|
||||
, m_accelerationLimitOn(true)
|
||||
, m_accelerationLimit(50.0f)
|
||||
, m_accelerationLimit(100.0f)
|
||||
, m_stepsPerRevolution(200)
|
||||
, m_microStepResolution(8)
|
||||
, m_currentPeriod(0)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user