#ifndef _DRIVING_H_
#define _DRIVING_H_

#define FORWARD  1
#define BACKWARD 0
#define ACROSS   2
#define GOAL     3
#define DEPOSIT  4
#define FLAGFORWARD 5
#define FLAGACROSS  6
#define FLAGBOX      7
#define FLAGREVERSE 8
#define DEADRECKONING 9
#define FLAGREPEAT   10

#define RightMotor 0
#define LeftMotor 1

#define TargetVelocityStraight 110
#define MaxVelocity 180

#define TargetVelocityTurn 100

#define RightEncoder 25
#define LeftEncoder 24

float ki;
float kp;
float kd;

float EncoderDifferential(); 

void drive_straight(int16_t, uint32_t,int8_t,int16_t);

void ForwardApplyMV(float);
void BacwardApplyMV(float);

float BoundedVelocity(float);
void SetRightVelocityforward(float);
void SetLeftVelocityforward(float);
void SetRightVelocitybackward(float);
void SetLeftVelocitybackward(float);

void ResetEncoders();
void CoastMotors();
void CoastMotor(uint8_t);

#endif