unkown work UART 2

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->Throttle = 1;
-  pPacket->Steering = 0;
+  pPacket->Steering = 2;
-  pPacket->Kp = 0;
+  pPacket->Kp = 3;
-  pPacket->Ki = 0;
+  pPacket->Ki = 4;
-  pPacket->Kd = 0;
+  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
   }

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
+// MemoryPool<RCProtocol::Packet, 16> mpool;
-SPI spi(PA_7, PA_6, PA_5);   //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
+// Queue<RCProtocol::Packet, 16> RCQueue;
-mpu6000_spi imu(spi,PA_4);   //define the mpu6000 object
 
 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
+DigitalOut motorL(PC_8);
-InterruptIn gyroINT(PC_4);
+DigitalOut motorR(PC_6);
-
-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()...
-    }
-}
-
 
+//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;
+    // Thread readUartThread;
-    readUartThread.start(callback(&serialContext));
+    // readUartThread.start(callback(&serialContext));
-
+    //serial.set_dma_usage_rx(DMA_USAGE_ALWAYS);
-    // MPU startup at 100hz
+    //serial.attach(&serialContext);
-    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);

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)