From e0648f23abba4010f73666fb882c36a3e306aa37 Mon Sep 17 00:00:00 2001 From: philsson Date: Mon, 22 Jun 2020 17:37:51 +0200 Subject: [PATCH] unkown work UART 2 --- NodeMCU/OSCWifiBridge/OSCWifiBridge.ino | 14 +- main.cpp | 242 ++++++------------------ src/drivers/stepper.cpp | 2 +- 3 files changed, 71 insertions(+), 187 deletions(-) diff --git a/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino b/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino index 2cf8a17..aafa55b 100644 --- a/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino +++ b/NodeMCU/OSCWifiBridge/OSCWifiBridge.ino @@ -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 } diff --git a/main.cpp b/main.cpp index de58ee9..2d5591a 100644 --- a/main.cpp +++ b/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 mpool; +// Queue 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); diff --git a/src/drivers/stepper.cpp b/src/drivers/stepper.cpp index 9558797..8d30f76 100644 --- a/src/drivers/stepper.cpp +++ b/src/drivers/stepper.cpp @@ -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)