agent-pid-ga.cpp

#include <stdio.h>
#include "bobuniverse.h"
#include <vector>

#define INITIALDISTANCE 60
#define SETPOINT 30
#define AUTOMATICSTOP 200
#define GENERATIONS 500
#define INDIVIDUALS 40
#define CROMO 4
#define GENES 4
#define FLOATADJUST 10000000.0
#define MAXVALUE 10*FLOATADJUST
#define KP 6
#define KI 0.03
#define KD 0.7
int main(void)
{
    std::vector<Universe> myBob(INDIVIDUALS, Universe(INITIALDISTANCE, SETPOINT));
    int distance, time, status, power, sum, error, delta = 0, speed, lasterror = 0, counter = 0;
    status = myBob[0].getStatus();
    while(status == 1 && counter < AUTOMATICSTOP)
    {
        distance = myBob[0].getDistance();
        error = distance - SETPOINT;
        sum = sum + error;

        if(error == 0 || (error >= 0 && lasterror <= 0) || (error <= 0 && lasterror >= 0))
        {
            sum = 0;
        }

        /*          P           I           D     */
        power = (error*KP) + (sum*KI) + (delta*KD);
        myBob[0].move(power);
        status = myBob[0].getStatus();
        counter++;
        printf("distance: %d error: %d sum: %d delta: %d power: %d \n", distance, error, sum, delta, power);
        delta = error - lasterror;
        lasterror = error;
    }
    status = myBob[0].getStatus();
    distance = myBob[0].getDistance();
    speed = myBob[0].getSpeed();
    time = myBob[0].getTime();
    printf("finished with distance %d speed %d time: %d status: %d\n", distance, speed, time, status);
    return 0;
}

linkpralib.txt

https://github.com/OtacilioN/Bob-Robot-Simulator