void TeleopPeriodic()
{
static int targetSpeed = 0;
targetSpeed+= stick->GetY()*-3;
static int fakerange = 0;
if (stick->GetRawButton(2))
{
targetSpeed=0;
fakerange = 250;
}
if (stick->GetRawButton(3))
{
targetSpeed += 150;
}
if (stick->GetRawButton(4))
{
targetSpeed -= 150;
}
if(stick->GetRawButton(5))
{
fakerange+=3;
}
if(stick->GetRawButton(6))
{
fakerange-=3;
}
launch->Obey();
launch->SetTargetSpeed(targetSpeed);
float currAngle= angle->PIDGet();
float range = fakerange*cos(currAngle*3.1415/180);
std::cout<<"range estimate = "<<range<<std::endl;
launch->Aim(range/100.0);
int current =rightWheel->GetEncVel();
std::cout<<targetSpeed<<" "<<current<<std::endl;
}
