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; }