Challenges using Controls

Setting up PID Controller

Driving based on angular velocity

  1. Setup controller config struct
  2. Init i2c first
  3. Choose values for controller config
  4. Create controller class based on config
  5. Setup controller/timing variables
  6. Turn on pico led last
  7. Inside while loop:
    1. Update current time
    2. Check if timestep has passed
    3. Get sensor data
    4. Calculate controller output based on desired value
    5. Set motor power based on controller output

    #include "rcc_stdlib.h"
    using namespace std;
    
    //setup instance of config struct
    PID_control_config_t config;
    
    int main(){
        stdio_init_all();
        cyw43_arch_init();
        sleep_ms(1500);
    
        //init i2c first
        rcc_init_i2c(); 
        MPU6050 imu;
        imu.begin(i2c1);
        imu.calibrate();
    
        Motor motors;
        MotorInit(&motors, RCC_ENA, RCC_ENB, 20000);
        MotorsOn(&motors);
    
        //set values for config
        config.kp = 1.5; 
        config.ki = 0;
        config.kd = 0;
        config.ts = 0.01; // 1ms
        config.sigma = 0.1;
        config.lowerLimit = -50;
        config.upperLimit = 50;
        config.antiWindupEnabled = true;
    
        //create controller class based on config
        PID_control controller(config);
    
        //motor power variables setup
        int base_power = 50;
        int output; 
    
        //timing variables setup
        uint32_t current_time, previous_time;
        current_time = time_us_32();
    
        //pico led on when done setup
        cyw43_arch_gpio_put(0,1); 
    
        while(true){
            //update current time
            current_time = time_us_32();
    
            //if config's timestep has passed
            if((current_time - previous_time) >= config.ts*1e6){
    
                //get data from sensors
                imu.update_pico();
                //calculate controller output:
                    //first input is desired value
                    //second input is actual value
                output = controller.pid(0.0, imu.getAngVelZ());
    
                //set motor power based on controller output
                MotorPower(&motors, (base_power-output), (base_power + output));

                //reset previous time
                previous_time = current_time;
            }
        }
    }

Challenges

★ Travel straight for a distance

★★ Path Following

★★ Orientation Controller

★★★ Play Fetch🐾

★★★ Path Following

★★★ Travel in a Square

★★★ Parking Between Objects