/*
Make the motors spin on the KeyStudio KS0193 self balancing car.
You need to turn on the switch on the side, so the 18650 batteries are connected
or the motor does not have power to turn.

We use the LEDC PWM of the ESP32 and the TB6612 H-Bridge to drive the motors.
 */
//TB6612 pins
const int right_R1=D8;    
const int right_R2=D12;
const int PWM_R=D10;
const int left_L1=D7;
const int left_L2=D6;
const int PWM_L=D9;

// use first channel of 16 channels (started from zero)
#define LEDC_CHANNEL_0     0 // for right motor
#define LEDC_CHANNEL_1     1 // for left motor

// use 12 bit precission for LEDC timer
#define LEDC_TIMER_12_BIT  12

// use this Hz as a LEDC base frequency
#define LEDC_BASE_FREQ     19531

int slowness = 0;    // how slow the motor is (255 is the slowest)
int fadeAmount = 1;    // how many points to slow the motor by

// Arduino like analogWrite
// value has to be between 0 and valueMax
void ledcAnalogWrite(uint8_t channel, uint32_t value, uint32_t valueMax = 255) {
  // calculate duty, 4095 from 2 ^ 12 - 1
  uint32_t duty = (4095 / valueMax) * min(value, valueMax);

  // write duty to LEDC
  ledcWrite(channel, duty);
}

void setup() {
  Serial.begin(115200);
  pinMode(right_R1,OUTPUT);     // set all TB6612pins to OUTPUT
  pinMode(right_R2,OUTPUT);
  pinMode(PWM_R,OUTPUT);
  pinMode(left_L1,OUTPUT);
  pinMode(left_L2,OUTPUT);
  pinMode(PWM_L,OUTPUT);

  // Setup timer and attach timer to a PWM pins
  if (ledcSetup(!LEDC_CHANNEL_0, LEDC_BASE_FREQ, LEDC_TIMER_12_BIT)){
    Serial.println("Problems!");
  }
  ledcAttachPin(PWM_R, LEDC_CHANNEL_0);

  if (ledcSetup(!LEDC_CHANNEL_1, LEDC_BASE_FREQ, LEDC_TIMER_12_BIT)){
    Serial.println("Problems!");
  }
  ledcAttachPin(PWM_L, LEDC_CHANNEL_1);
}

void loop() {
  digitalWrite(right_R1,HIGH);
  digitalWrite(right_R2,LOW);
  

  digitalWrite(left_L1,HIGH);
  digitalWrite(left_L2,LOW);

  // set the slowness on LEDC channel 0
  ledcAnalogWrite(LEDC_CHANNEL_0, slowness);
  // set the slowness on LEDC channel 1
  ledcAnalogWrite(LEDC_CHANNEL_1, slowness);

  // change the slowness for next time through the loop:
  slowness = slowness + fadeAmount;

  // reverse the direction of the fading at the ends of the fade:
  if (slowness < 1 || slowness >= 250) {
    fadeAmount = -fadeAmount;
  }
  // wait for 30 milliseconds to see the dimming effect
  delay(10);

}