mirror of
https://github.com/team2059/Dent
synced 2024-12-18 20:52:29 -05:00
Added mechanum drive code (untested)
This commit is contained in:
parent
d925e5c08f
commit
734399f344
@ -1,20 +1,32 @@
|
||||
#include "HHRobot.h"
|
||||
#include "HHBase.h"
|
||||
HHRobot::HHRobot():
|
||||
//
|
||||
hhdrive(new RobotDrive(2,0,3,1)),
|
||||
gyro(new Gyro(1)),
|
||||
joystick1(new Extreme3dPro(0)){
|
||||
hhdrive->SetExpiration(0.1);
|
||||
hhdrive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
|
||||
hhdrive->SetInvertedMotor(RobotDrive::kRearLeftMotor,true);
|
||||
hhdrive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
|
||||
hhdrive->SetInvertedMotor(RobotDrive::kRearLeftMotor,true);
|
||||
}
|
||||
void HHRobot::Init(){
|
||||
printf("Initing\n");
|
||||
printf("Code Version: %f\n",0000.1);
|
||||
gyro->Reset();
|
||||
//Put table values initally to avoid annoying refreshing
|
||||
}
|
||||
//Main function used to handle periodic tasks on the robot
|
||||
void HHRobot::Handler(){
|
||||
hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("x"));
|
||||
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"));
|
||||
}
|
||||
Wait(0.005);
|
||||
// hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("x"));
|
||||
}
|
||||
// vim: ts=2:sw=2:et
|
||||
|
@ -6,6 +6,7 @@
|
||||
class HHRobot{
|
||||
private:
|
||||
RobotDrive *hhdrive;
|
||||
Gyro *gyro;
|
||||
Extreme3dPro *joystick1;
|
||||
public:
|
||||
HHRobot();
|
||||
|
Loading…
Reference in New Issue
Block a user