CartBotArduino/CartBotArduino.ino at main · frc3468/CartBotArduino · 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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
/*
  #include <SPI.h>
  #include <Ethernet.h>
  #include <EEPROM.h>
  #include <Servo.h>
*/

#include <RobotOpenHA.h>

#define ANALOG_RESOLUTION 1023 // 10-bit

#define HW_MAX 255 // 1.0
#define HW_NEUTRAL 128 // 0.0
#define HW_MIN 0 //-1.0

#define SERVO_MIN 10
#define SERVO_MAX 245

#define SW_MAX 127 // 1.0
#define SW_NEUTRAL 0 // 0.0
#define SW_MIN -128//-1.0

// Controllers
const uint8_t driverControllerUSB = 1;

// PWM + Digital
// ARDUINO PINS 0 & 1 ARE RESERVED FOR THE SERIAL INTERFACE!!!
// ARDUINO PIN 10 IS RESERVED FOR THE ETHERNET CONTROLLER!!!!!
const uint8_t leftFrontDriveMotorsPWM = 2;
const uint8_t leftRearDriveMotorsPWM = 3;
const uint8_t rightFrontDriveMotorsPWM = 4;
const uint8_t rightRearDriveMotorsPWM = 5;

// Analog
const uint8_t voltageDividerAnalog = 0;


// I/O Objects
ROJoystick driverController(driverControllerUSB);

ROPWM leftFrontDriveMotor(leftFrontDriveMotorsPWM);
ROPWM leftRearDriveMotor(leftRearDriveMotorsPWM);
ROPWM rightFrontDriveMotor(rightFrontDriveMotorsPWM);
ROPWM rightRearDriveMotor(rightRearDriveMotorsPWM);

ROAnalog voltageDivider(voltageDividerAnalog);

// Drivetrain Variables
int8_t driveSpeedAxis() {
  return map(driverController.leftY(), HW_MIN, HW_MAX, SW_MAX, SW_MIN); /* inverted */
}
int8_t driveRotationAxis() {
  return map(driverController.leftX(), HW_MIN, HW_MAX, SW_MIN, SW_MAX);
}

const int8_t driveMaxSpeed = 80;
const int8_t driveMinSpeed = -1*driveMaxSpeed;
const uint8_t driveDeadzone = 10;

const uint8_t motorMaxSpeed = map(driveMaxSpeed, SW_MIN, SW_MAX, HW_MIN, HW_MAX);
const uint8_t motorMinSpeed = map(driveMinSpeed, SW_MIN, SW_MAX, HW_MIN, HW_MAX);

// Voltage Divider Variables
const uint8_t voltageDividerMaxReading = 25; // volts
const float voltageDividerStep = (float)voltageDividerMaxReading / ANALOG_RESOLUTION;


void setup()
{
  /* Initiate comms */
  RobotOpen.begin(&enabled, &disabled, &timedtasks);

  leftFrontDriveMotor.attach();
  leftRearDriveMotor.attach();
  rightFrontDriveMotor.attach();
  rightRearDriveMotor.attach();
}


/* This is your primary robot loop - all of your code
   should live here that allows the robot to operate
*/
void enabled() {
  // Drivetrain Control
  int8_t driveSpeed = driveSpeedAxis();
  int8_t driveRotation = driveRotationAxis();

  // Joystick Deadzones
  if (abs(driveSpeed) <= driveDeadzone) {
    driveSpeed = SW_NEUTRAL;
  }
  if (abs(driveRotation) <= driveDeadzone) {
    driveRotation = SW_NEUTRAL;
  }

  int8_t leftDriveSpeed = constrain(driveSpeed - driveRotation, SW_MIN, SW_MAX);
  int8_t rightDriveSpeed = constrain(driveSpeed + driveRotation, SW_MIN, SW_MAX);

  uint8_t leftMotorOutput = map(leftDriveSpeed,SW_MIN, SW_MAX, motorMaxSpeed, motorMinSpeed); /* Inverted */
  uint8_t rightMotorOutput = map(rightDriveSpeed, SW_MIN, SW_MAX, motorMinSpeed, motorMaxSpeed);

  leftFrontDriveMotor.write(leftMotorOutput);
  leftRearDriveMotor.write(leftMotorOutput);
  rightFrontDriveMotor.write(rightMotorOutput);
  rightRearDriveMotor.write(rightMotorOutput);
}


/* This is called while the robot is disabled */
void disabled() {
  // safety code

  // Neutral-Out PWMs
  leftFrontDriveMotor.write(HW_NEUTRAL);
  leftRearDriveMotor.write(HW_NEUTRAL);
  rightFrontDriveMotor.write(HW_NEUTRAL);
  rightRearDriveMotor.write(HW_NEUTRAL);
}


/* This loop ALWAYS runs - only place code here that can run during a disabled state
   This is also a good spot to put driver station publish code
*/
void timedtasks() {
  RODashboard.publish("Uptime (s)", ROStatus.uptimeSeconds());
  RODashboard.publish("Batt Voltage (v)", (voltageDivider.read()*voltageDividerStep));
}


// !!! DO NOT MODIFY !!!
void loop() {
  RobotOpen.syncDS();
}