main.cpp

#include <iostream>
#include <math>
#include <algorithm>

class PID {
private: 
	// define gains:
	// propotional, integral, derivative 
	double Kp, Ki, Kd;

	double integral;
	double previous_speed;

public: 
	// default constructor (no PID control)
	PID() {
		Kp = 0;
		Ki = 0; 
		Kd = 0;
	}
	// consttructur set to desired gains
	PID(double Kp, double Ki, double Kd) {
		Kp = kp;
		Ki = ki;
		Kd = kd;
		previous_speed = 0;
		integral = 0;
	}
	// Set the speed from 0 to current speed
	void setspeed(double current_speed) {
		previous_speed = current_speed;
	}
	//calculate the command change
	//need target speed, current speed, and time step to take
	double math(double target_speed, double current_speed, double dt) {
		double true_error = target_speed - current_speed;
		double integral += (current_speed - previous_speed) * dt; // SUM(dv*dt)
		double derivative = -1*(current_speed - previous_speed) / dt; //  dv/dt
		double output_percent = Kp*true_error+Ki*integral+Kd*derivative);
		previous_speed = current_speed;  //set speed
		return output_percent // returns a percentage output
	}

};

double CAN_IN_FORWARD_VELOCITY() {};
double CAN_OUT_THROTTLE(double throttle_percent) {};
bool READ_CRUISE_BUTTON() {};
double READ_TARGET_SPEED() {};
//double CAN_OUT_REGEN() {};


int main() {

	PID CruisePID(1, 0, 0); //set kp, ki, kd --> tune

	double target_speed; 
	double actual_speed;
	bool CruiseON;
	double percent_throttle;
	double step = 0.02;

	while (1) {

		CruiseON = READ_CRUISE_BUTTON();
		target_speed = READ_TARGET_SPEED();

		if (CruiseON) {
			CruisePID.setspeed(CAN_input_actual_speed);
			percent_throttle = CruisePID.math(input_target_speed, CAN_input_actual_speed, step);
			std::clamp(percent_throttle, 0, 100);
			
			//maybe
			/*if (percent_throttle < 0) {
				CAN_OUT_REGEN(percent_throttle);
				}*/

			CAN_OUT_THROTTLE(percent_throttle);
			}
		}
	}

	
}