sumo.c

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/adc.h"

// motors
// high high front
const uint8_t PIN_ML_EN = 9;
const uint8_t PIN_ML_PH = 10;
const uint8_t PIN_ML_SLP = 11;
uint slice_num_ML = 0;
uint16_t val_pwm_ML = 0;
bool dir_ML = true;

const uint8_t PIN_MR_EN = 6;
const uint8_t PIN_MR_PH = 7;
const uint8_t PIN_MR_SLP = 8;
uint slice_num_MR = 0;
uint16_t val_pwm_MR = 0;
bool dir_MR = true;

const uint16_t PWM_WRAP = 1250;

const uint16_t pwm_min = 300;
const uint16_t pwm_max = 800;
const float amplitude = (pwm_max - pwm_min) / 2.0f;
const float offset = pwm_min + amplitude;

const float frequency = 1.0f;


// mux
const uint8_t PIN_MULT_A0 = 14;
const uint8_t PIN_MULT_A1 = 12;
const uint8_t PIN_MULT_A2 = 13;
const uint8_t PIN_MULT_EN = 15;
const uint8_t PIN_MULT_D = 29;
const uint8_t MULT_BITS_NUM = 3;
typedef enum Sensors
{
    ENEMY_FRONT,
    ENEMY_BACK,
    LINE_BACK,
    LINE_FRONT
} Sensors;
Sensors curr_sensor = LINE_FRONT;

uint8_t PIN_MULT_BITS_INIT[3] = {PIN_MULT_A0, PIN_MULT_A1, PIN_MULT_A2};
const uint8_t PIN_MULT_BITS[4][3] = { {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {0, 0, 1}};

uint16_t adc_result_enemy_front;
uint16_t adc_result_enemy_back;
uint16_t adc_result_line_front;
uint16_t adc_result_line_back;
bool on_white_line = false;

// start module
const uint8_t PIN_START_MODULE = 0;

// misc
const bool LOW = 0;
const bool HIGH = 1;
const uint16_t VAL_WHITE_LINE = 3500;
const uint16_t VAL_BLACK_LINE = 3700;
bool reversed = false;
uint8_t cnt_white_line = 0;
uint8_t cnt_black_line = 0;
const uint8_t NUM_SAMPLES = 100;
bool mux_busy = false;
volatile bool mux_ready = false;
const uint16_t MIN_ENEMY_DISTANCE = 4000;
bool reversing_while_attack = false;

// timers
volatile bool line_front_sens_ready = true;
volatile bool line_back_sens_ready = true;
volatile bool enemy_front_sens_ready = true;
volatile bool enemy_back_sens_ready = true;
const uint16_t MUX_WAIT_TIME = 100;
const uint16_t LINE_WAIT_TIME = 500;
const uint16_t ENEMY_WAIT_TIME = 50;

// states
typedef enum States
{
    IDLE,
    WAKE_UP,
    FIND_ENEMY,
    CHANGE_DIR,
    ATTACK_FRONT,
    ATTACK_FRONT_LINE,
    ATTACK_BACK,
    ATTACK_BACK_LINE,
    FAST_SEARCH,
} States;
States curr_state = IDLE;

// funcs
void init_motors();
void init_mux();
void init_start_module();
void wake_up();
int64_t line_front_sensor_alarm_callback(alarm_id_t id, void *user_data);
int64_t line_back_sensor_alarm_callback(alarm_id_t id, void *user_data);
int64_t enemy_front_sensor_alarm_callback(alarm_id_t id, void *user_data);
int64_t enemy_back_sensor_alarm_callback(alarm_id_t id, void *user_data);
int64_t enemy_on_line_alarm_callback(alarm_id_t id, void *user_data);
int64_t mux_alarm_callback(alarm_id_t id, void *user_data);
void set_mux(uint8_t bit1, uint8_t bit2, uint8_t bit3);
Sensors get_curr_sensor(Sensors curr_sensor);
bool is_on_line();


int main()
{
    // init
    stdio_init_all();
    init_motors();
    init_mux();
    init_start_module();

    while (true) 
    {

        switch (curr_state)
        {
            case IDLE:
                if (!gpio_get(PIN_START_MODULE))
                {
                    curr_state = WAKE_UP;
                }
            break;
            

            case WAKE_UP:
                wake_up();
                curr_state = FIND_ENEMY;
            break;


            case FIND_ENEMY:
                uint64_t sin_timestamp = time_us_64();
                float t = sin_timestamp / 1e6f;
                float sin_val_L = sinf(2.0f * M_PI * frequency * t);
                float sin_val_R = sinf(2.0f * M_PI * frequency * t + M_PI);
                val_pwm_ML = (uint16_t)(offset + amplitude * sin_val_L);
                val_pwm_MR = (uint16_t)(offset + amplitude * sin_val_R);

                pwm_set_chan_level(slice_num_ML, PWM_CHAN_B, val_pwm_ML);
                pwm_set_chan_level(slice_num_MR, PWM_CHAN_A, val_pwm_MR);
                
                // on white line
                // TODO: DODAC LINE_FRONT
                on_white_line = is_on_line();
                
                if (on_white_line && !reversed)
                {
                    curr_state = CHANGE_DIR;
                }
                else if (!on_white_line && reversed)
                {
                    reversed = false;
                }
                
                // enemy front front: high high
                if (adc_result_enemy_front >= MIN_ENEMY_DISTANCE)
                {
                    curr_state = ATTACK_FRONT;
                }
                else if (adc_result_enemy_back >= MIN_ENEMY_DISTANCE)
                {
                    curr_state = ATTACK_BACK;
                }
            break;


            case CHANGE_DIR:
                dir_ML = !dir_ML;
                dir_MR = !dir_MR;
                gpio_put(PIN_ML_PH, dir_ML);
                gpio_put(PIN_MR_PH, dir_MR);
                reversed = true;
                curr_state = FIND_ENEMY;

            break;

            case ATTACK_FRONT:
                gpio_put(PIN_MR_PH, HIGH);
                gpio_put(PIN_ML_PH, HIGH);
                dir_ML = HIGH;
                dir_MR = HIGH;
                val_pwm_ML = PWM_WRAP;
                val_pwm_MR = PWM_WRAP;
                pwm_set_chan_level(slice_num_ML, PWM_CHAN_B, val_pwm_ML);
                pwm_set_chan_level(slice_num_MR, PWM_CHAN_A, val_pwm_MR);

                if (adc_result_enemy_back >= MIN_ENEMY_DISTANCE)
                {
                    curr_state = ATTACK_BACK;
                }
                else if (adc_result_enemy_front < MIN_ENEMY_DISTANCE)
                {
                    curr_state = FIND_ENEMY;
                }
                else if (adc_result_line_back < VAL_WHITE_LINE) // DODAC CZUJNIK LINE FRONT
                {
                    curr_state = ATTACK_FRONT_LINE;
                }
            break;

            case ATTACK_BACK:
                gpio_put(PIN_MR_PH, LOW);
                gpio_put(PIN_ML_PH, LOW);
                dir_ML = LOW;
                dir_MR = LOW;
                val_pwm_ML = PWM_WRAP;
                val_pwm_MR = PWM_WRAP;
                pwm_set_chan_level(slice_num_ML, PWM_CHAN_B, val_pwm_ML);
                pwm_set_chan_level(slice_num_MR, PWM_CHAN_A, val_pwm_MR);

                if (adc_result_enemy_front >= MIN_ENEMY_DISTANCE)
                {
                    curr_state = ATTACK_FRONT;
                }
                else if (adc_result_enemy_back < MIN_ENEMY_DISTANCE)
                {
                    curr_state = FIND_ENEMY;
                }
                else if (adc_result_line_back < VAL_WHITE_LINE) // TODO: DODAC LINE FRONT
                {
                    curr_state = ATTACK_BACK_LINE;
                }
            break;

            case ATTACK_FRONT_LINE:
                gpio_put(PIN_MR_PH, LOW);
                gpio_put(PIN_ML_PH, LOW);
                dir_ML = LOW;
                dir_MR = LOW;    
            if (adc_result_line_back >= VAL_BLACK_LINE)// TODO: front sensor
                {
                    curr_state = ATTACK_FRONT;
                }
            break;

            case ATTACK_BACK_LINE:
                gpio_put(PIN_MR_PH, HIGH);
                gpio_put(PIN_ML_PH, HIGH);
                dir_ML = HIGH;
                dir_MR = HIGH;
                if (adc_result_line_back >= VAL_BLACK_LINE)
                {
                    curr_state = ATTACK_BACK;
                }
            break;

            default:
                break;
        }

        // change sensor, read val
        if (enemy_back_sens_ready && !mux_busy)
        {
            set_mux(PIN_MULT_BITS[ENEMY_BACK][0], PIN_MULT_BITS[ENEMY_BACK][1], PIN_MULT_BITS[ENEMY_BACK][2]);
            mux_busy = true;
            mux_ready = false;
            add_alarm_in_us(MUX_WAIT_TIME, mux_alarm_callback, NULL, false);
            curr_sensor = ENEMY_BACK;
        }
        if (enemy_front_sens_ready && !mux_busy)
        {
            set_mux(PIN_MULT_BITS[ENEMY_FRONT][0], PIN_MULT_BITS[ENEMY_FRONT][1], PIN_MULT_BITS[ENEMY_FRONT][2]);
            mux_busy = true;
            mux_ready = false;
            add_alarm_in_us(MUX_WAIT_TIME, mux_alarm_callback, NULL, false);
            curr_sensor = ENEMY_FRONT;
        }
        if (line_back_sens_ready && !mux_busy)
        {
            set_mux(PIN_MULT_BITS[LINE_BACK][0], PIN_MULT_BITS[LINE_BACK][1], PIN_MULT_BITS[LINE_BACK][2]);
            mux_busy = true;
            mux_ready = false;
            add_alarm_in_us(MUX_WAIT_TIME, mux_alarm_callback, NULL, false);
            curr_sensor = LINE_BACK;
        }
        if (line_front_sens_ready && !mux_busy)
        {
            set_mux(PIN_MULT_BITS[LINE_FRONT][0], PIN_MULT_BITS[LINE_FRONT][1], PIN_MULT_BITS[LINE_FRONT][2]);
            mux_busy = true;
            mux_ready = false;
            add_alarm_in_us(MUX_WAIT_TIME, mux_alarm_callback, NULL, false);
            curr_sensor = LINE_FRONT;
        }
        if (mux_ready)
        {
            switch (curr_sensor)
            {
                case LINE_BACK:
                    adc_result_line_back = adc_read();
                    add_alarm_in_us(LINE_WAIT_TIME, line_back_sensor_alarm_callback, NULL, false);
                    line_back_sens_ready = false;
                    mux_ready = true;
                    mux_busy = false;
                    break;

                case LINE_FRONT:
                    adc_result_line_front = adc_read();
                    add_alarm_in_us(LINE_WAIT_TIME, line_front_sensor_alarm_callback, NULL, false);
                    line_front_sens_ready = false;
                    mux_ready = true;
                    mux_busy = false;
                    break;

                case ENEMY_FRONT:
                    adc_result_enemy_front = adc_read();
                    add_alarm_in_ms(ENEMY_WAIT_TIME, enemy_front_sensor_alarm_callback, NULL, false);
                    enemy_front_sens_ready = false;
                    mux_ready = true;
                    mux_busy = false;
                    break;

                case ENEMY_BACK:
                    adc_result_enemy_back = adc_read();
                    add_alarm_in_ms(ENEMY_WAIT_TIME, enemy_back_sensor_alarm_callback, NULL, false);
                    enemy_back_sens_ready = false;
                    mux_ready = true;
                    mux_busy = false;
                    break;

                default:
                    break;
            }
        }
        
        printf("STATE: %d, M1: %d, M2: %d, ADC_F: %d, ADC_B: %d, LINE_F: %d\n", curr_state, dir_ML, dir_MR, adc_result_enemy_front, adc_result_enemy_back, adc_result_line_back);
    }
}

void init_motors()
{
    
    // 1 motor driver
    gpio_init(PIN_ML_PH);
    gpio_init(PIN_ML_SLP);
    gpio_set_dir(PIN_ML_PH, GPIO_OUT);
    gpio_set_dir(PIN_ML_SLP, GPIO_OUT);
    gpio_put(PIN_ML_PH, LOW);
    gpio_put(PIN_ML_SLP, LOW);
    gpio_set_function(PIN_ML_EN, GPIO_FUNC_PWM);
    slice_num_ML = pwm_gpio_to_slice_num(PIN_ML_EN);
    pwm_set_enabled(slice_num_ML, true);
    pwm_set_wrap(slice_num_ML, PWM_WRAP);
    pwm_set_chan_level(slice_num_ML, PWM_CHAN_B, LOW);
    val_pwm_ML = 0;

    // sterownik silnika 2
    gpio_init(PIN_MR_PH);
    gpio_init(PIN_MR_SLP);
    gpio_set_dir(PIN_MR_PH, GPIO_OUT);
    gpio_set_dir(PIN_MR_SLP, GPIO_OUT);
    gpio_put(PIN_MR_PH, LOW);
    gpio_put(PIN_MR_SLP, LOW);
    gpio_set_function(PIN_MR_EN, GPIO_FUNC_PWM);
    slice_num_MR = pwm_gpio_to_slice_num(PIN_MR_EN);
    pwm_set_enabled(slice_num_MR, true);
    pwm_set_wrap(slice_num_MR, PWM_WRAP);
    pwm_set_chan_level(slice_num_MR, PWM_CHAN_A, LOW);
    val_pwm_MR = 0;
}

void init_mux()
{
    for (uint8_t i = 0; i < MULT_BITS_NUM; i++)
    {
        gpio_init(PIN_MULT_BITS_INIT[i]);
        gpio_set_dir(PIN_MULT_BITS_INIT[i], GPIO_OUT);
    }
    gpio_init(PIN_MULT_EN);
    gpio_set_dir(PIN_MULT_EN, GPIO_OUT);
    gpio_put(PIN_MULT_EN, 0);

    adc_init();
    adc_gpio_init(PIN_MULT_D);
    adc_select_input(3);
}

void init_start_module()
{
    gpio_init(PIN_START_MODULE);
    gpio_set_dir(PIN_START_MODULE, GPIO_IN);
    gpio_pull_up(PIN_START_MODULE);
}

void wake_up()
{
    // left motor driver
    gpio_put(PIN_ML_PH, HIGH);
    gpio_put(PIN_ML_SLP, HIGH);

    // right motor driver
    gpio_put(PIN_MR_PH, HIGH);
    gpio_put(PIN_MR_SLP, HIGH);

    // mux
    gpio_put(PIN_MULT_EN, 1);
}

int64_t line_front_sensor_alarm_callback(alarm_id_t id, void *user_data)
{
    line_front_sens_ready = true;
    return 0;
}

int64_t line_back_sensor_alarm_callback(alarm_id_t id, void *user_data)
{
    line_back_sens_ready = true;
    return 0;
}

int64_t enemy_front_sensor_alarm_callback(alarm_id_t id, void *user_data)
{
    enemy_front_sens_ready = true;
    return 0;
}

int64_t enemy_back_sensor_alarm_callback(alarm_id_t id, void *user_data)
{
    enemy_back_sens_ready = true;
    return 0;
}

int64_t enemy_on_line_alarm_callback(alarm_id_t id, void *user_data)
{
    reversing_while_attack = false;
    return 0;
}

int64_t mux_alarm_callback(alarm_id_t id, void *user_data)
{
    mux_ready = true;
    return 0;
}

void set_mux(uint8_t bit1, uint8_t bit2, uint8_t bit3)
{
    gpio_put(PIN_MULT_BITS_INIT[0], bit3);
    gpio_put(PIN_MULT_BITS_INIT[1], bit2);
    gpio_put(PIN_MULT_BITS_INIT[2], bit1);
}

Sensors get_curr_sensor(Sensors curr_sensor)
{
    if (curr_sensor == LINE_BACK)
        return ENEMY_FRONT;
    else
        return ++curr_sensor;
}

bool is_on_line()
{
    if (adc_result_line_back <= VAL_WHITE_LINE)
    {
        ++cnt_white_line;
    }
    else if (adc_result_line_back > VAL_BLACK_LINE)
    {
        ++cnt_black_line;
    }

    if (cnt_white_line + cnt_black_line >= NUM_SAMPLES)
    {
        if (cnt_white_line > cnt_black_line)
        {
            return true;
        }
        else if (cnt_white_line < cnt_black_line)
        {
            return false;
        }
        cnt_white_line = 0;
        cnt_black_line = 0;
    }
    return on_white_line;
}