/*
  Name:    setCurrent.ino
  Created: 19-08-2018
  Author:  SolidGeek
  Description: This is a very simple example of how to set the current for the motor
*/
#include <Arduino.h>
#include "VescUart.h"

/** Initiate VescUart class */
VescUart vesc;

/** Initiate SoftwareSerial class */
 int TX1 = 26;
 int RX1 = 25;

 int TX2 = 32;
  int RX2 = 33;

  int motor_current;
  int motor_rpm;
  float battery_voltage;
  double battery_current;


float current = 1.0; /** The current in amps */


void vesc_send_get_value() {

    byte message[] = {2, 1, 4, 64, 132, 3, 251};

    Serial1.write(message, sizeof(message));
    

}




void get_vesc_values() {
  /** Call the function getVescValues() to acquire data from VESC */
  if (vesc.getVescValues() ) {

     motor_current = vesc.data.avgMotorCurrent;
     motor_rpm = vesc.data.rpm;
     battery_voltage = vesc.data.inpVoltage;
    //float battery_amp_hours = vesc.data.ampHours;
    battery_current = vesc.data.avgInputCurrent;



    Serial.print("Battery Voltage :");
    Serial.print(battery_voltage);
    Serial.print(" ");
    Serial.print("Battery Current :");
    Serial.print(battery_current);
    Serial.print(" ");
    Serial.print("Motor RPM :");
    Serial.print(motor_rpm);
    Serial.print(" ");
    Serial.print("Motor Current :");
    Serial.println(motor_current);
    Serial.println(" ");
    Serial.print("Get FW");
   // Serial.println(vesc.getFirmwareVersion());
    
   

  }
  else
  {
    Serial.println("Failed to get data!");
  }

 
}


void set_duty_cycle(float duty_cycle, int test_duration) {

  static bool test_cycle = false;
  static float start_duty_cycle = 0.0;
  static int i = 0;

if (test_cycle == false) {

  while (i < test_duration) {

  vesc.setDuty(start_duty_cycle);
  start_duty_cycle = duty_cycle;
  vesc.sendKeepalive();
  Serial2.print("Duty Cycle: ");
  Serial2.print(start_duty_cycle);
  Serial2.println(F(""));
  Serial2.print("Test Duration: ");
  Serial2.print(i);
  Serial2.print(F("/"));
  Serial2.print(test_duration);
  Serial2.println(F(""));
  i++;
  delay(150);

  if (i >= test_duration) {
    test_cycle = true;
      Serial2.println("Test Complete");

  vesc.setDuty(0.0);
  }



}

} else if (test_cycle == true) {


  vesc.setDuty(0.0);
 // Serial2.print("Duty Cycle: ");
 // Serial2.print(start_duty_cycle);
 // Serial2.println(F(""));
 }


}



void set_vesc_current() {

  static bool test_cycle = false;
static float current = 0.0;


if (current < 20.0) {
 
  //  vesc.printVescValues();
   // vesc.setDutyCycle(current);
    vesc.setCurrent(current);
    current = current + 0.01;

    Serial2.println("Current: " + String(current));  
 // current++;

} else {
  current = 0.0;

  delay(5000);
}

}



void setup() {

  /** Setup Serial port to display data */
  Serial.begin(115200); // ESP8266 (NodeMCU
  Serial1.setRxBufferSize(512); // increasing buffer size ?

  Serial1.begin(115200, SERIAL_8N1, RX1, TX1); 

  Serial2.begin(115200, SERIAL_8N1, RX2, TX2);

   /** Define which ports to use as UART */
  vesc.setSerialPort(&Serial1);
  vesc.setDebugPort(&Serial2);

  //set_duty_cycle(0.5); 
}

void loop() {
//  set_vesc_current();
 // vesc.sendKeepalive();
  //vesc_send_get_value();  
   // vesc_get_value();
     set_duty_cycle(0.0, 1000); 

}