2
0
mirror of https://github.com/team2059/Dent synced 2024-12-18 20:52:29 -05:00
dent/src/HHRobot.cpp

45 lines
1.7 KiB
C++
Raw Normal View History

2015-01-05 19:07:24 -05:00
#include "HHRobot.h"
#include "HHBase.h"
HHRobot::HHRobot():
hhdrive(new RobotDrive(2,0,3,1)),
2015-01-09 20:20:05 -05:00
gyro(new Gyro(1)),
2015-01-09 20:57:26 -05:00
collector(new DentCollector(4, 5, 6, 7)),
2015-01-05 19:07:24 -05:00
joystick1(new Extreme3dPro(0)){
hhdrive->SetExpiration(0.1);
2015-01-09 20:20:05 -05:00
hhdrive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
hhdrive->SetInvertedMotor(RobotDrive::kRearLeftMotor,true);
2015-01-05 19:07:24 -05:00
}
void HHRobot::Init(){
printf("Initing\n");
printf("Code Version: %f\n",0000.1);
2015-01-09 20:20:05 -05:00
gyro->Reset();
2015-01-05 19:07:24 -05:00
//Put table values initally to avoid annoying refreshing
}
//Main function used to handle periodic tasks on the robot
void HHRobot::Handler(){
2015-01-09 20:20:05 -05:00
const float Kp = 0.3;
if(joystick1->GetJoystickButton(1)==1){
hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), 0, joystick1->GetJoystickAxis("x"));
}else if(joystick1->GetJoystickButton(2)==1){
hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), 0);
}else if(joystick1->GetJoystickButton(3)==1){
hhdrive->Drive(joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("y")*Kp*-gyro->GetAngle());
}else{
hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("x"));
}
2015-01-09 20:57:26 -05:00
if(joystick1->GetJoystickButton(11)){
2015-01-10 11:46:30 -05:00
collector->Collect(-1.0f);
2015-01-09 20:57:26 -05:00
}else if(joystick1->GetJoystickButton(12)){
2015-01-10 11:46:30 -05:00
collector->Collect(1.0f);
2015-01-09 20:57:26 -05:00
}else if(joystick1->GetJoystickButton(9)){
collector->Raise(255);
}else if(joystick1->GetJoystickButton(10)){
collector->Raise(1);
}else{
collector->Rest();
}
2015-01-09 20:20:05 -05:00
Wait(0.005);
// hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("x"));
2015-01-05 19:07:24 -05:00
}
// vim: ts=2:sw=2:et