team2059/dent
2
0
Fork 0
mirror of https://github.com/team2059/Dent synced 2025-01-07 22:14:14 -05:00
Code Issues Releases Wiki Activity

Added mechanum drive code (untested)

Browse Source
This commit is contained in:
Austen Adler 2015-01-09 20:20:05 -05:00
parent d925e5c08f
commit 734399f344
2 changed files with 17 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/HHRobot.cpp
View File

@ -1,20 +1,32 @@
#include "HHRobot.h" #include "HHRobot.h"
#include "HHBase.h" #include "HHBase.h"
HHRobot::HHRobot(): HHRobot::HHRobot():
//
hhdrive(new RobotDrive(2,0,3,1)), hhdrive(new RobotDrive(2,0,3,1)),
gyro(new Gyro(1)),
joystick1(new Extreme3dPro(0)){ joystick1(new Extreme3dPro(0)){
hhdrive->SetExpiration(0.1); hhdrive->SetExpiration(0.1);
hhdrive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true); hhdrive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
hhdrive->SetInvertedMotor(RobotDrive::kRearLeftMotor,true); hhdrive->SetInvertedMotor(RobotDrive::kRearLeftMotor,true);
} }
void HHRobot::Init(){ void HHRobot::Init(){
printf("Initing\n"); printf("Initing\n");
printf("Code Version: %f\n",0000.1); printf("Code Version: %f\n",0000.1);
gyro->Reset();
//Put table values initally to avoid annoying refreshing //Put table values initally to avoid annoying refreshing
} }
//Main function used to handle periodic tasks on the robot //Main function used to handle periodic tasks on the robot
void HHRobot::Handler(){ 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 // vim: ts=2:sw=2:et

1
src/HHRobot.h
View File

@ -6,6 +6,7 @@
class HHRobot{ class HHRobot{
private: private:
RobotDrive *hhdrive; RobotDrive *hhdrive;
Gyro *gyro;
Extreme3dPro *joystick1; Extreme3dPro *joystick1;
public: public:
HHRobot(); HHRobot();