Skip to content
Snippets Groups Projects
Select Git revision
  • main default
1 result
Blame Permalink
Code owners
Assign users and groups as approvers for specific file changes. Learn more.
main.c 14.78 KiB 
#include <pico/stdlib.h>
#include <hardware/i2c.h>
#include <stdio.h>
#include <unistd.h>
#include <hardware/pwm.h>
#include <math.h>

double rollingperc[4] = {0, 0, 0, 0};
int pos = 0;

const uint8_t init[4] = {0x21, 0xa0, 0xef, 0x81};
const uint8_t zeile[8] = {0x00, 0x02, 0x04, 0x06, 0x08, 0x10, 0x12, 0x14};
const uint8_t fill[17] = {0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};

const uint8_t empty[17] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
// erste byte: bei welche zeile es anfängt
// 0x00 => data address pointer command begins with 0000, second half modifies command to tell which line it is
/*
lines go from 0 - 15 => nimm alle gerade (0, 2, 4, 6, 8, 10, 12, 14), (1, 3, 5, 7, 9, 11, 13, 15)

so every other byte is egal

0xff == every led in row lights up (all 8 bits are lit up)

*/
const uint8_t matrix[9][17] = {
    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // empty
    {0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // achtel
    {0x00, 0xff, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // viertel
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // dreiachtel
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // hälfte
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // fünfachtel
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00}, // dreiviertel
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0x00, 0x00}, // siebenachtel
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00}, // ganzes
};
const uint8_t better_matrix[65][17] = {
    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},

    {0x00, 0b00000001, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0b00000011, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0b00000111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0b00001111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0b00011111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0b00111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0b01111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0b11111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},

    {0x00, 0xff, 0x00, 0b00000001, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0b00000011, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0b00000111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0b00001111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0b00011111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0b00111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0b01111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0b11111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},

    {0x00, 0xff, 0x00, 0xff, 0x00, 0b00000001, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0b00000011, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0b00000111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0b00001111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0b00011111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0b00111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0b01111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0b11111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},

    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000001, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000011, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00001111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00011111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b01111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b11111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},

    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000001, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000011, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00001111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00011111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b01111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b11111111, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},

    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000001, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000011, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000111, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00001111, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00011111, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00111111, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b01111111, 0x00, 0x00, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b11111111, 0x00, 0x00, 0x00, 0x00, 0x00},

    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000001, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000011, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000111, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00001111, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00011111, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00111111, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b01111111, 0x00, 0x00, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b11111111, 0x00, 0x00, 0x00},

    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000001, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000011, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00000111, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00001111, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00011111, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b00111111, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b01111111, 0x00},
    {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b11111111, 0x00},
};

void motor_forward()
{
    gpio_put(20, 1);
    gpio_put(21, 0);
}

void motor_backward()
{
    gpio_put(20, 0);
    gpio_put(21, 1);
}

void set_speed(double speed)
{
    volatile uint slice_num = pwm_gpio_to_slice_num(19);
    pwm_set_chan_level(slice_num, PWM_CHAN_B, speed);
}
void set_progress(double percentage)
{

    int numerator = (percentage * 32) / 100;

    i2c_write_blocking(i2c_default, 0x70, better_matrix[numerator], 17, false);

    // for (int i = 0; i <= numerator; i++)
    //{
    //    i2c_write_blocking(i2c_default, 0x70, better_matrix[i], 17, false);
    //}
}

void callback(uint gpio, uint32_t events)

{

    volatile uint slice_num = pwm_gpio_to_slice_num(27);

    if (events & GPIO_IRQ_EDGE_FALL)
    {

        volatile uint16_t duration = pwm_get_counter(slice_num);

        volatile float distance = (duration * 340) / (20000);
        if (distance <= 2)
        {
            distance = 0;
        }
        if (distance > 20)
        {
            distance = 20;
        }

        distance = 20 - distance;

        volatile int percentage = (distance / 10) * 100;

        rollingperc[pos] = percentage;
        pos++;
        if (pos > 3)
        {
            pos = 0;
        }

        int avg = (rollingperc[0] + rollingperc[1] + rollingperc[2] + rollingperc[3]) / 4;
        // if zwischen 90, 110, stop

        double speed = 0.5 * ((avg - 100) * (avg - 100)) + 1000; // quadratisch
        speed = speed + 10 * ((1.8 * avg) - 180);

        // double speed = abs(70 * avg) + 2000; // linear
        // speed += 70;

        set_speed(speed);
        if (avg == 100)
        {
            gpio_put(20, 0);
            gpio_put(21, 0);
        }
        if (avg < 100)
        {
            motor_forward();
        }
        else if (avg > 100)
        {
            motor_backward();
        }
        set_progress(avg);
    }

    else if (events & GPIO_IRQ_EDGE_RISE)
    {
        pwm_set_counter(slice_num, 0);
    }
}

void init_pwm()
{
    gpio_init(28);
    gpio_set_dir(28, GPIO_OUT);

    gpio_set_function(28, GPIO_FUNC_PWM);

    volatile uint slice_num = pwm_gpio_to_slice_num(28);
    pwm_set_clkdiv(slice_num, 125);
    //
    pwm_set_wrap(slice_num, 0xFFFF);
    pwm_set_chan_level(slice_num, PWM_CHAN_A, 10);
    pwm_set_enabled(slice_num, true);
}

void pwm_echo_init()
{
    gpio_init(27);
    gpio_set_dir(27, GPIO_IN);

    volatile uint slice_num = pwm_gpio_to_slice_num(27);
    /*
    // Count once for every 100 cycles the PWM B input is high
    25 pwm_config cfg = pwm_get_default_config();
    26 pwm_config_set_clkdiv_mode(&cfg, PWM_DIV_B_HIGH);
    27 pwm_config_set_clkdiv(&cfg, 100);
    28 pwm_init(slice_num, &cfg, false);
    29 gpio_set_function(gpio, GPIO_FUNC_PWM);
    */
    // count once for every 42 cycles the PWM B input is high
    // one cycle approximately equal to 20 ms
    pwm_set_clkdiv(slice_num, 125);
    pwm_set_wrap(slice_num, 0xFFFF);
    pwm_set_clkdiv_mode(slice_num, PWM_DIV_FREE_RUNNING);
    pwm_set_enabled(slice_num, true);
}

void init_motor()
{
    gpio_init(19);
    gpio_set_dir(19, GPIO_OUT);

    gpio_set_function(19, GPIO_FUNC_PWM);

    gpio_init(20);
    gpio_set_dir(20, GPIO_OUT);

    gpio_init(21);
    gpio_set_dir(21, GPIO_OUT);

    volatile uint slice_num = pwm_gpio_to_slice_num(19);

    pwm_set_clkdiv(slice_num, 100);                  // 100 -> 1.25 Mhz
    pwm_set_wrap(slice_num, 10000);                  // 10000 -> 10k * 1mhz = 10k microseconds = 10 ms total
    pwm_set_chan_level(slice_num, PWM_CHAN_B, 2500); // 2000 - 10000 = HIGH FOR 3 MILLISECONDS
    // 3 High, 7 low, 3 High

    // 50 Hz für langsam, 100 für schnell
    pwm_set_enabled(slice_num, true);
    // gpio_put(19, 1);
}

int main()
{
    // clock enable: 100.000 baud (bit/s)
    i2c_init(i2c_default, 1000000);
    // Pin 0 and 1 are given I2C functionality (they're the ones connected to the led matrix)
    gpio_set_function(0, GPIO_FUNC_I2C);
    gpio_set_function(1, GPIO_FUNC_I2C);

    // refer to page 439 in RP2040, redundant with write_blocking - pins are used as output (write enable) (sort of)
    // gpio_pull_up(1);
    // gpio_pull_up(0); // sets to 3.3 / 5 V, i2c sets it to 0

    // 0x70 i2c address for 28 pin (refer to matrix.pdf page 21)
    // can't be directly written cuz we need pointers
    // system setup (clock enable), ROW/INT set, dimming, display setup
    // const uint8_t init[4] = {0x21, 0xa0, 0xe1, 0x81};

    i2c_write_blocking(i2c_default, 0x70, &init[0], 1, false);
    i2c_write_blocking(i2c_default, 0x70, &init[1], 1, false);
    i2c_write_blocking(i2c_default, 0x70, &init[2], 1, false);
    i2c_write_blocking(i2c_default, 0x70, &init[3], 1, false);

    init_pwm();
    pwm_echo_init();
    init_motor();

    gpio_set_irq_enabled_with_callback(27, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, true, callback);

    /*
    timer_hw->dbgpause = 0;
    gpio_init(27);
    gpio_init(28);

    gpio_set_dir(28, GPIO_OUT);
    while (1)
    {
        gpio_put(28, true);
        sleep_us(10);
        gpio_put(28, false);
        while (!gpio_get(27))
            ;
        volatile uint32_t start = time_us_32();
        while (gpio_get(27))
            ;
        volatile uint32_t end = time_us_32();

        uint32_t duration = end - start;
        volatile float distance = (duration * 314) / (20000);
        if (distance > 20)
        {
            distance = 20;
        }

        distance = 20 - distance;

        volatile int percentage = (distance / 10) * 100;

        set_progress(percentage);
        asm("BKPT #0");
    }
    */
    while (1)
    {
    }
    return 0;
}