TechBot2014/DrivingBase.cpp at master · team3990/TechBot2014 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
/*----------------------------------------------------------------------------*/
/* Copyright (c) TechForKid 2014. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/

#include "DrivingBase.h"

/* --------------------------------------------------------------------------- */
// Constructors
DrivingBase::DrivingBase()
: m_Shifter(C_cRIOPneumaticModuleNumber, C_SolenoidShifter),
  m_motorLeft(C_MotorLeftFront),
  m_motorRight(C_MotorRightFront)
{
  // Log to console
  printf("DrivingBase Constructor Started\n");

  // Properties
  m_Seuil_Rotation    = 0.0; // (0.1?)
  m_Seuil_Forward     = 0.0; // (0.3?)
  m_CompensationFriction_Forward  = 0; // (15?)
  m_CompensationFriction_Rotation = 0; // (20?)

  // Create a robot using standard left/right robot drive
  m_moteurs = new RobotDrive(&m_motorLeft, &m_motorRight);
  m_motorLeft.EnableDeadbandElimination(true);
  m_motorRight.EnableDeadbandElimination(true);
}
DrivingBase::~DrivingBase()
{

}

/* --------------------------------------------------------------------------- */
// Methods

//
// SamrtDashboard functions
//
void DrivingBase::DashboardInit()
{
  // Send default values to populate dashboard
  SmartDashboard::PutNumber("Forward compensation",       m_CompensationFriction_Forward);
  SmartDashboard::PutNumber("Rotation compensation",      m_CompensationFriction_Rotation);
  SmartDashboard::PutNumber("Max Forward compensation",   m_Seuil_Forward);
  SmartDashboard::PutNumber("Min Rotation compensation",  m_Seuil_Rotation);
}

void DrivingBase::UpdateDashboardInputs()
{
  //Send dashboard values
  m_CompensationFriction_Forward  = SmartDashboard::GetNumber("Forward compensation");
  m_CompensationFriction_Rotation = SmartDashboard::GetNumber("Rotation compensation");
  m_Seuil_Forward                 = SmartDashboard::GetNumber("Max Forward compensation");
  m_Seuil_Rotation                = SmartDashboard::GetNumber("Min Rotation compensation");
}

void DrivingBase::Drive(float iForward, float iRotation)
{
  // Pour compenser la friction quand on tourne à une petite vitesse
/*  if (iForward < m_Seuil_Forward && iForward > -m_Seuil_Forward              &&
      (iRotation > m_Seuil_Rotation || iRotation < -m_Seuil_Rotation) )
  {
    m_motorLeft.SetBounds (206, 128+m_CompensationFriction_Rotation, 128, 128-m_CompensationFriction_Rotation, 56);
    m_motorRight.SetBounds(206, 128+m_CompensationFriction_Rotation, 128, 128-m_CompensationFriction_Rotation, 56);
  }
  else
  {
    m_motorLeft.SetBounds (206, 128+m_CompensationFriction_Forward,  128, 128-m_CompensationFriction_Forward,  56);
    m_motorRight.SetBounds(206, 128+m_CompensationFriction_Forward,  128, 128-m_CompensationFriction_Forward,  56);
  }
»*/
  m_moteurs->ArcadeDrive(-iForward, iRotation);
}

void DrivingBase::HighSpeed()
{
  m_Shifter.Set(true);
}

void DrivingBase::LowSpeed()
{
  m_Shifter.Set(false);
}

void DrivingBase::ToggleSpeed()
{
  if(m_Shifter.Get() == true)
  {
    m_Shifter.Set(false);
  }
  else
  {
    m_Shifter.Set(true);
  }
}