# <!-- # # const p_gain = 0.0;
# # # const i_gain = 0.0;
# # # const d_gain = 0.0;

# # # const set_point = 50;
# # # const max_output = 500;
# # # const base_speed = 500;

# # # let i_memory = 0.0;
# # # let last_error = 0.0;

# # # const motorA = ev3_motorA(); // Left motor
# # # const motorB = ev3_motorB(); // Right motor
# # # const colour = ev3_colorSensor();

# # # let left_speed = 0.0;
# # # let right_speed = 0.0;

# # # function setup() {
# # # display(ev3_connected(motorA) ? "A connected" : "A not connected");
# # # display(ev3_connected(motorB) ? "B connected" : "B not connected");
# # # display(ev3_connected(colour) ? "colour connected" : "colour not connected");

# # # ev3_motorSetSpeed(motorA, base_speed);
# # # ev3_motorSetSpeed(motorB, base_speed);

# # # ev3_motorSetStopAction(motorA, "brake");
# # # ev3_motorSetStopAction(motorB, "brake");

# # # ev3_pause(2000);
# # # ev3_playSequence([440, 1000, 0]);
# # # display("Setup done!");
# # # }

# # # function loop() {
# # # ev3_motorStart(motorA);
# # # ev3_motorStart(motorB);

# # # while (ev3_colorSensorGetColor(colour) !== 5) { // if not red
# # # const error = ev3_reflectedLightIntensity(colour) - set_point;

# # # i_memory = i_memory + i_gain * error;
# # # if (i_memory > max_output) { i_memory = max_output; }
# # # else if (i_memory < max_output * -1) { i_memory = max_output * -1; }
# # # else {}
# # # // display(i_memory, "I gain memory: ");

# # # let output = p_gain * error +
# # # i_memory +
# # # d_gain * (error - last_error);

# # # if (output > max_output) { output = max_output; }
# # # else if (output < max_output * -1) { output = max_output * -1; }
# # # else {}
# # # // display(output, "PID output: ");

# # # if (output < 0) {
# # # // Turn LEFT
# # # left_speed = base_speed + output;
# # # right_speed = base_speed;
# # # } else {
# # # // Turn RIGHT
# # # left_speed = base_speed;
# # # right_speed = base_speed - output;
# # # }
# # # // display(left_speed, " Left: ", right_speed, "Right: ");

# # # ev3_motorSetSpeed(motorA, left_speed);
# # # ev3_motorSetSpeed(motorB, right_speed);
# # # ev3_motorStart(motorA);
# # # ev3_motorStart(motorB);

# # # last_error = error;
# # # ev3_pause(10);
# # # }
# # # ev3_motorStop(motorA);
# # # ev3_motorStop(motorB);
# # # }

# # # setup();
# # # loop(); -->

# Const Set point: The vale we want to tend towards and track (Reflected light intesity)
# Let Current point: Current value
# Let Difference: Difference between set and current

# Proportional tuning: Y = mx + c

# Corrective action to be exerted is proportional to the difference we need to figure out our m and c

# Integral tuning: Increases the corrective action to be exerted to reach set point more quickly. Cons is that the resultant change calculated can overshoot the difference.

# Derivative tuning: Proportional to the change of the change of current point.

# Issue at hand:

# Set point: Range - 0 to 70
# 10
# Difference = Current - Set point
# => If too left, current value < set point therefore difference is negative.
# Else too right, difference is positive.

# Corrective action: If too left, turn right, less power to right wheel
# If too right, turn left, more power to right wheel

# Integral tuning: Proportional to the difference.
# If more absolute difference, increase change.

# Derivative tuning: Proportional to the change of the change.
# If current change is going to overshoot in this amount of loops, decrease change. If still going to overshoot, decrease change somemore.

# Values to consider:

# Set point
# Current point
# Difference

# absolute change
# Change
# I_change

# future point

# Output = Proportional + Integral + Derivative
# Proportional = Kp * (target - value)
# // set initial Kp value first and the Ki and Kd as 0 and fine tune from there
# // Small Kp smoother turns
# // Large Kp sharper turns
# Integral = Ki * (Integral + error)
# Derivative = Kd * (error - last_error)
# // Small Kd larger loop
# // Large Kd smaller loop

# // All 3 K values are arbitrary and require fine-tuning

# ```javascript=
# const motorA = ev3_motorA();
# const motorB = ev3_motorB();
# const colour = ev3_colorSensor();

# const set_point = ev3_reflectedLightIntensity(colorSensor);

# // is it i am not sure halp

# let current_point = set_point;
# let difference = current_point - set_point;

# function wide_turn(dir) {
# const motor_dur = 300
# const decrement = 500;
# const base_speed = 1000;
# if (dir === "left") {
# ev3_runForTime(motorA, motor_dur, base_speed - decrement);
# ev3_runForTime(motorB, motor_dur, base_speed);
# } else if(dir === "right") {
# ev3_runForTime(motorA, motor_dur, base_speed);
# ev3_runForTime(motorB, motor_dur, base_speed - dec);
# }
# }

# function narrow_turn () {
# motor_dur = 300;
# const motor_dur = 300
# const decrement = 100;
# const base_speed = 1000;
# if (dir === "left") {
# ev3_runForTime(motorA, motor_dur, base_speed - decrement);
# ev3_runForTime(motorB, motor_dur, base_speed);
# } else if(dir === "right") {
# ev3_runForTime(motorA, motor_dur, base_speed);
# ev3_runForTime(motorB, motor_dur, base_speed - decrement);
# }
# }

# }

# function integral() {
# const significant_difference = 5;
# if (difference >= abs(significant_difference)) {
# wide_turn()
# } else if (difference < abs(significant_difference) {
# narrow_turn();
# }
# }

# while (ev3_colorSensorGetColor(color) !== 5) {

# }
# function loop() {

# }

const motorA = ev3_motorA();
const motorB = ev3_motorB();
const colour = ev3_colorSensor();

const set_point = ev3_reflectedLightIntensity(colour);
ev3_motorSetStopAction(motorA, "brake");
ev3_motorSetStopAction(motorB, "brake");

// is it i am not sure halp

// let current_point = set_point;
// let difference = current_point - set_point; 
// let previous_diff = current_point - set_point;
// let previous_dir = "right";
// let motor_dur = 100;

// function wide_turn(dir) {
//     const motor_dur = 100;
//     const decrement = 500;
//     const base_speed = 1000;
//     if (dir === "left") {
//         ev3_runForTime(motorA, motor_dur, base_speed - decrement);
//         ev3_runForTime(motorB, motor_dur, base_speed);
//     } else if(dir === "right") {
//         ev3_runForTime(motorA, motor_dur, base_speed);
//         ev3_runForTime(motorB, motor_dur, base_speed - decrement);
//     } else{}
// }

// function narrow_turn(dir) {
//      motor_dur = 300;
//      const motor_dur = 300
//      const decrement = 100;
//      const base_speed = 1000;
//      if (dir === "left") {
//          ev3_runForTime(motorA, motor_dur, base_speed - decrement);
//          ev3_runForTime(motorB, motor_dur, base_speed);
//      } else if(dir === "right") {
//          ev3_runForTime(motorA, motor_dur, base_speed);
//          ev3_runForTime(motorB, motor_dur, base_speed - decrement);
//      }
//  }

// function integral() {
//     const significant_difference = 5;
//     current_point = ev3_reflectedLightIntensity(colorSensor); 
//     difference = current_point - set_point; 
//     if (difference <= previous_diff) {
//         if (difference >= significant_difference) {
//             wide_turn("left");
//             previous_dir = "left";
//             previous_diff = difference;
//         } else if (difference < significant_difference) {
//             narrow_turn("left");
//             previous_dir = "left";
//             previous_diff = difference;
//         } else {}
//     } else {
//         if (difference >= significant_difference) {
//             wide_turn("right");
//             previous_dir = "right";
//             previous_diff = difference;
//         } else if (difference < significant_difference) {
//             narrow_turn("right");
//             previous_dir = "right";
//             previous_diff = difference;
//         } else {}
//     }
// }

// ev3_motorSetStopAction(motorA, break);
// ev3_motorSetStopAction(motorB, break);


// function spiral() {
//     const motor_dur = 100;
//     const base_speed = 300;
    
//     ev3_runForTime(motorA, motor_dur, base_speed);
//     ev3_runForTime(motorB, motor_dur, base_speed);
    
    
//     while (ev3_colorSensorGetColor(color) !== 5) {
//         integral(); 
//     } 
    
//     ev3_motorStop(motorA); 
//     ev3_motorStop(motorB);
    
// }



// SUBMISSION ====================================================================================================

/*
BENEDICTcustodIANs

Ang Hui Shan Angela
Chen Hsiao Ting
Chen Shi Yao, Stephanie
Chia Ann Yao Eugene
Jeremy Yeo Zhi Chen
Kalinga Mudalige Don Sachin David
Sherman Ng Wei Sheng
Tan Zheng Jie, Dickson
*/

const motorA = ev3_motorA();
const motorB = ev3_motorD();
const colour = ev3_colorSensor();

const set_point = ev3_reflectedLightIntensity(colour);


let current_point = set_point;
let difference = current_point - set_point; 
let previous_diff = current_point - set_point;
let previous_dir = "right";
let motor_dur = 100;


//For when the difference is large
function wide_turn(dir) {
    const motor_dur = 10;
    const decrement = 900;
    const base_speed = - 300;
    if (dir === "left") {
        ev3_runForTime(motorA, motor_dur, base_speed - decrement);
        ev3_runForTime(motorB, motor_dur, base_speed);
    } else if(dir === "right") {
        ev3_runForTime(motorA, motor_dur, base_speed);
        ev3_runForTime(motorB, motor_dur, base_speed - decrement);
    } else{}
}

//For when the difference is small
//decrement reduces speed of one motor to make robot turn in the 
//desired direction
function narrow_turn(dir) {
     const motor_dur = 100;
     const decrement = 700;
     const base_speed = - 200;
     if (dir === "left") {
         ev3_runForTime(motorA, motor_dur, base_speed - decrement);
         ev3_runForTime(motorB, motor_dur, base_speed);
     } else if(dir === "right") {
         ev3_runForTime(motorA, motor_dur, base_speed);
         ev3_runForTime(motorB, motor_dur, base_speed - decrement);
     } else {}
 }

function integral() {
    const significant_difference = 15;
    current_point = ev3_reflectedLightIntensity(colour); 
    difference = current_point - set_point; 
    if (difference <= previous_diff) {
        if (difference >= significant_difference) {
            wide_turn("right");
            previous_dir = "right";
            previous_diff = difference;
        } else if (difference < significant_difference) {
            narrow_turn("right");
            previous_dir = "right";
            previous_diff = difference;
        } else {}
    } else {
        if (difference >= significant_difference) {
            wide_turn("left");
            previous_dir = "left";
            previous_diff = difference;
        } else if (difference < significant_difference) {
            narrow_turn("left");
            previous_dir = "left";
            previous_diff = difference;
        } else {}
    }
}

function spiral() {
    const motor_dur = 100;
    const base_speed = -10;
    
    ev3_runForTime(motorA, motor_dur, base_speed);
    ev3_runForTime(motorB, motor_dur, base_speed);
    
    
    while (ev3_reflectedLightIntensity(colour) < 20) {
        integral(); 
    } 
}

// Function application to execute PID mission
spiral();




// sos();
// ev3_pause(5000);


// function sos() {
    
//     let A = -300;
//     let B = -800;
//     while (ev3_colorSensorGetColor(colour) !== 5) {
//         ev3_runForTime(motorA, 400, A);
//         ev3_runForTime(motorB, 400, B);
//         ev3_pause(400);
        
//         A = A + 30;
//         B = B - 10;
//     }
    
// }
Ok

I think we need a function to go the ther way if the difference is increasing to drasticallly

change exerted must be drastic
and do we have a derivative tuning thingy?