/* ***********************************************************
 * Company: FATEC Sto Andre
 * Autor:   Paulo
 * Date: 02/01/2025
 * Processador: RP2040
 * Software License Agreement: Somente para fins didaticos
 ************************************************************
 * File Description: Template padrao para programa em C SDK.
 * Change History:
 * 1.0 02/01/2025
***********************************************************/
#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/adc.h"
#include "hardware/pwm.h"

#define ADC_NUM 2
#define ADC_PIN (26 + ADC_NUM)
#define PWM_0_PIN 14

const int targetPosition = 90;  // desired target position      
const float dt = 0.02;         
const float Kp = 0.5;          
const float Ki = 0.2;          
const float Kd = 0.02;         

float integral = 0;
float previousError = 0;

int main() {
  stdio_init_all();
  adc_init();
  adc_gpio_init( ADC_PIN);
  adc_select_input( ADC_NUM);

  uint16_t adc_raw, pwm_duty, sensor, sensor_value;
    // Tell GPIO they are allocated to the PWM
    gpio_set_function(PWM_0_PIN, GPIO_FUNC_PWM);
    // Find out which PWM slice is connected to GPIO
    uint slice_num = pwm_gpio_to_slice_num(PWM_0_PIN);
    // pwm freq
    pwm_set_clkdiv(slice_num, 12.5); // pwm clock should now be running at 1MHz // 12.5 -FREQ: 100MHz
    // Set period of 100 cycles (0 to 99 inclusive)
    pwm_set_wrap(slice_num, 99);
    // Set channel A output high for one cycle before dropping
    // PWM set to 30%
    pwm_set_chan_level(slice_num, PWM_CHAN_A, 0);
    // Set the PWM running
    pwm_set_enabled(slice_num, true);

    while(1){
        adc_select_input(ADC_NUM);
        adc_raw = adc_read();  // raw digital from ADC
        adc_select_input(ADC_NUM - 1);
        sensor = adc_read();  // raw digital from ADC
        float currentPosition = sensor;  // Read the current position of the servo
        float error = targetPosition - currentPosition;
        // Calculate the PID components
        float proportional = Kp * error;
        integral += Ki * error * dt;
        float derivative = Kd * (error - previousError) / dt;
        // Calculate the control output
        float output = proportional + integral + derivative;
        pwm_duty = (adc_raw * 100)/4095;
        // Apply the control output
        pwm_set_chan_level(slice_num, PWM_CHAN_A, pwm_duty + output);
        previousError = error;
        sleep_ms(dt * 1000);  // Convert time step to milliseconds and wait
    }
}