/**********************************************************************
* Wheelchair Kit Firmware
* Created By: Tysen Marshall and Ky ashcraft
* 
*
*
*
**********************************************************************/

#define R_EN 7
#define L_EN 8
#define RPWM 9   //forward
#define LPWM 10     //reverse
const int yPin = A1;  //y axis of joystick

//functions for setting pwm duty with new frequency 
//pin9 -> OCR1A
//pin10 -> OCR1B
void setDutyRPWM(float percent) {
  percent = constrain(percent, 0, 100); //ensures percent is in the allowable range before chaning register
  OCR1A = (uint16_t)((percent / 100.0) * (ICR1 + 1));
}
void setDutyLPWM(float percent) {
  percent = constrain(percent, 0, 100);  
  OCR1B = (uint16_t)((percent / 100) * (ICR1 + 1));
}

unsigned long previousMillis = 0; //stores last time updated pwm
const long interval = 1000; //interval in milliseconds

void setup() {

Serial.begin(9600);
delay(500);
Serial.println("connected...");

pinMode(RPWM, OUTPUT);
pinMode(LPWM, OUTPUT);
pinMode(R_EN, OUTPUT);
pinMode(L_EN, OUTPUT);

//clear timer 1 registers
TCCR1A = 0;
TCCR1B = 0;
TCNT1 = 0;  //reset timer count

//set top value of timer counter
// Top Value = (Frequency of CPU / (prescaler * Desired PWM Frequency)) -1
//eg: (16,000,000 / (1 * 25,000)) - 1
ICR1 = 639; 

//select fast pwm mode and sets the top defined by ICR1
TCCR1A |= (1 << WGM11);
TCCR1B |= (1 << WGM13) | (1 << WGM12);

//set non inverting pwm
TCCR1A |= (1 << COM1A1) | (1 << COM1B1);

//set prescaler to 1 allows for highest resolution
TCCR1B |= (1 << CS10);

//enable forward and rear movement
digitalWrite(R_EN, HIGH); 
digitalWrite(L_EN, HIGH);

}

void loop() {
  unsigned long currentMillis = millis();
  /*ky's joystick code here*/

  /*ky's battery monitoring code here*/




  if(currentMillis - previousMillis >= interval) {

    int joystick = analogRead(yPin);
    int center = 600; //joystick center
    //Serial.println(joystick);
    int deadzone = 5;  //change if there is drift when joystick shouldnt be giving input
    //potentially not needed if ky's code calls out the variable

    if (joystick < center) {
      joystick = map(joystick, 0, 600, -100, 0);
    } else if (joystick >= center) { 
      joystick = map(joystick, 600, 1023, 0, 100);
    }
    if (abs(joystick) < deadzone) joystick = 0;
    joystick = constrain(joystick, -100, 100);
    Serial.println(joystick);

    //add input smoothing and failsafe for if adc of joystick is out of range
    //add ramp up/ramp down of motor

    

  //*/    //main joystick logic
    if (joystick < 0) {
      setDutyLPWM(0);
      setDutyRPWM(-joystick);   // negative is to give switch to a postive value for setdutyRPWM function
    }
    else if (joystick > 0){
      setDutyRPWM(0);
      setDutyLPWM(joystick); 
    }
    else if (joystick == 0){
      setDutyRPWM(0);
      setDutyLPWM(0);
    }
    else {
      Serial.println("Error: joystick input not valid");
    }
  //*/
  }
  

  //setDutyRPWM(10);


  /*    //joystick timeout
  //joystick timeout greater than 1 second all motors off
  if (millis() - lastJoystickUpdate > 1000) {
    setDutyRPWM(0);
    setDutyLPWM(0);
    Serial.println("Failsafe: no joystick update!");
  }

  */

  //Serial Debug output
  /*/
  Serial.println("joystick: "); Serial.print(joystick);
  Serial.println("    RPWM: "); Serial.print(OCR1A);
  Serial.println("    LPWM: "); Serial.print(OCR1B);   
  //*/ 
}