// Week 17; used this code to test power to the motors with just the RP2040 and 12V input and powerbank

#include <Arduino.h>

const int MOTOR_LEFT  = D3; // left motor
const int MOTOR_RIGHT = D9; // right motor


void setMotors(int leftSpeed, int rightSpeed, int max_value) {

  //leftSpeed  = constrain(leftSpeed, 0, 255); // makes sure the value is always [0,255]
  //rightSpeed = constrain(rightSpeed, 0, 255);

  analogWrite(MOTOR_LEFT, map(leftSpeed, 0, max_value, 0, max_value));
  analogWrite(MOTOR_RIGHT, map(rightSpeed, 0, max_value, 0, max_value));
}

void stopMotors() {
  analogWrite(MOTOR_LEFT, 0);
  analogWrite(MOTOR_RIGHT, 0);
}

void setup() {

  Serial.begin(115200); // to communicate via USB with the computer

  pinMode(MOTOR_LEFT, OUTPUT);
  pinMode(MOTOR_RIGHT, OUTPUT);

  analogWriteFreq(20000);      // default: 20 kHz PWM, less audible motor whining
  stopMotors();
}

void setup_PWM (int resolution, int frequency) {

  Serial.print("Setting frequency to: ");
  Serial.println(frequency);
  analogWriteFreq(frequency);

  Serial.print("Setting resolution to: ");
  Serial.print(resolution);
  Serial.println(" bits");
  analogWriteResolution(resolution); // sets the resolution

}

void test_duration (int resolution, int frequency, int duration) {

  setup_PWM (resolution, frequency);

  int max_value = (2 << (resolution-1))-1; // example: 8 bits resolution will lead to a max valu of 2^8 - 1 = 255
  Serial.print("Max value: ");
  Serial.println(max_value);

  int value = 0;

  for (int power = 10; power <= 100; power = power + 10) {

    value = (max_value * power) / 100;
    Serial.print("Value set to: ");
    Serial.println(value);

    setMotors(value, value, max_value);
    delay(duration);
  }
}

void test_run (int resolution, int frequency) { // resolution in bits; default 8 bits

  setup_PWM (resolution, frequency);
  
  int max_value = (2 << (resolution-1))-1; // example: 8 bits resolution will lead to a max valu of 2^8 - 1 = 255
  Serial.print("Max value: ");
  Serial.println(max_value);
  Serial.println("Starting sequency");
  for (int speed = 0; speed <= max_value; speed++) {
    setMotors(speed, speed, max_value);
    Serial.print("Speed: ");
    Serial.println(speed);
    delay(100);
  }

  delay(5000);

  // stop
  Serial.println("stop the motors");
  stopMotors();
  delay(2000);
}

void loop() {

  // Experiment 1: use different frequencies
  // test_run (8, 20000);
  // test_run (8, 10000);
  // test_run (8, 5000);
  // test_run (8, 2500);
  // test_run (8, 1000);
  // test_run (8, 500);
  // test_run (8, 100);
  // test_run (8, 20);


  // Experiment 2: use 20Khz but 8 and 10 bits resolution
  // test_run (8, 20000);
  // test_run (10, 20000);

  // Experiment 3: use 20Khz, 8 bits resolution, stay 20 seconds at intervals of 10%
  // test_duration(8,20000,30000);

  // Experiment 4: use 20Khz, 8 bit resolution, keep it at 10% for one minute
  setup_PWM(8, 20000);
  test_run(8, 20000);

  // setMotors(50,50,255); // 20% of max capacity
  //delay(60000);
  //stopMotors();
  delay(2000);

}
