PID_Teste

#define SOFT 0
#define HARD 1

#include<stdio.h>
/* demo.c:  My first C program on a Linux */

struct error {
    float yaw;
    float pitch;
    float roll;
};

struct PID {
    float yaw;
    float pitch;
    float roll;
};

long map(long x, long in_min, long in_max, long out_min, long out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


int normalize(float value)
{
    int maxVal = 1000;
    value = (int) value;

    if (value > maxVal) {
        value = maxVal;
    } else if (value < 0) {
        value = 0;
    }

    return value;
}

int main(void)
{
    int rxRoll;
    int rxPitch;
    int rxYaw;
    int flightMode = HARD;

    if (flightMode == HARD)
    {
      rxRoll = map(1500, 1000, 2000, -45, 45);
      rxPitch = map(1500, 1000, 2000, -45, 45);   
      rxYaw = map(1540, 1000, 2000, -180, 180);
    }
    else
    {
      rxRoll = map(1500, 1000, 2000, -180, 180);
      rxPitch = map(1500, 1000, 2000, -180, 180);
      rxYaw = map(1540, 1000, 2000, -270, 270);
    }
    
    int throttle = map(1500, 1000, 2000, 0, 100);

    printf("rx %d %d %d %d \n", throttle, rxYaw, rxPitch, rxRoll);
    
    float cmd_motA = throttle;
    float cmd_motB = throttle;
    float cmd_motC = throttle;
    float cmd_motD = throttle;

    //Somatório dos erros
    struct error sError = { .yaw = 0, .pitch = 0, .roll = 0};

    //Valor desejado - angulo atual
    struct error anguloAtual = { .yaw = 0, .pitch = 0, .roll = 0};
    struct error anguloDesejado = { .yaw = rxYaw, .pitch = rxPitch, .roll = rxRoll};

    struct error errors = { 
        .yaw = anguloDesejado.yaw - anguloAtual.yaw,
        .pitch = anguloDesejado.pitch - anguloAtual.pitch,
        .roll = anguloDesejado.roll - anguloAtual.roll
    };
    
    struct error lastErrors;
    struct error deltaError;

    //Variáveis de ajuste
    struct PID Kp = { .yaw = 1, .pitch = 1, .roll = 1};
    struct PID Ki = { .yaw = 0, .pitch = 0, .roll = 0};
    struct PID Kd = { .yaw = 0, .pitch = 0, .roll = 0};
    
    if (throttle != 0) {
        // Calculate sum of errors : Integral coefficients
        sError.yaw += errors.yaw;
        sError.pitch += errors.pitch;
        sError.roll += errors.roll;

        // Calculate error delta : Derivative coefficients
        deltaError.yaw = errors.yaw - lastErrors.yaw;
        deltaError.pitch = errors.pitch - lastErrors.pitch;
        deltaError.roll = errors.roll - lastErrors.roll;

        // Save current error as lastErr for next time
        lastErrors.yaw = errors.yaw;
        lastErrors.pitch = errors.pitch;
        lastErrors.roll = errors.roll;

        // Yaw - Lacet (Z axis)
        int yaw_PID = (errors.yaw * Kp.yaw + sError.yaw * Ki.yaw + deltaError.yaw * Kd.yaw);
        cmd_motA -= yaw_PID;
        cmd_motB += yaw_PID;
        cmd_motC += yaw_PID;
        cmd_motD -= yaw_PID;

        // Pitch - Tangage (Y axis)
        int pitch_PID = (errors.pitch * Kp.pitch + sError.pitch * Ki.pitch + deltaError.pitch * Kd.pitch);
        cmd_motA -= pitch_PID;
        cmd_motB -= pitch_PID;
        cmd_motC += pitch_PID;
        cmd_motD += pitch_PID;

        // Roll - Roulis (X axis)
        int roll_PID = (errors.roll * Kp.roll + sError.roll * Ki.roll + deltaError.roll * Kd.roll);
        cmd_motA -= roll_PID;
        cmd_motB += roll_PID;
        cmd_motC += roll_PID;
        cmd_motD -= roll_PID;
    }

    // Apply speed for each motor
    printf("motorA throttle %d \n", normalize(cmd_motA));
    printf("motorB throttle %d \n", normalize(cmd_motB));
    printf("motorC throttle %d \n", normalize(cmd_motC));
    printf("motorD throttle %d \n", normalize(cmd_motD));


    return 0;
}