135 lines
5.1 KiB
C
135 lines
5.1 KiB
C
#pragma once
|
|
#include "packet.h"
|
|
#include <math.h>
|
|
#define _USE_MATH_DEFINES // To access M_PI constant for trig functions
|
|
|
|
extern packet_t pA, pB, safe;
|
|
extern packet_t *astate, *incoming;
|
|
extern comm_state cs;
|
|
extern char comm_ok;
|
|
extern long last_p;
|
|
|
|
/*
|
|
#define CLOCKWISE 0
|
|
#define ANTICLOCKWISE 1
|
|
*/
|
|
|
|
// Loop timing
|
|
#define LOOP_DELAY_MS 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
|
|
#define LOOP_DELAY_SECONDS ((float)LOOP_DELAY_MS / 1000.0f) // Previous number expressed in seconds
|
|
|
|
// Math things
|
|
#define DEGREES_PER_RADIAN (180.0 / M_PI)
|
|
#define RADIANS_PER_DEGREE (M_PI / 180.0)
|
|
#define max(x,y) ( (x) > (y) ? (x) : (y) )
|
|
#define min(x,y) ( (x) < (y) ? (x) : (y) )
|
|
|
|
#define MOTOR_MAX_POWER 127.0 // Highest value accepted by motor control functions
|
|
|
|
// Drive modes
|
|
#define DRIVE_STOP 0
|
|
#define DRIVE_BASIC 1
|
|
#define DRIVE_TRANSLATION 2
|
|
#define DRIVE_ROTATION 3
|
|
|
|
// Controller maximum inputs for joystick
|
|
#define CONTROLLER_JOYSTICK_MAX 128.0
|
|
|
|
// Basic mode conditions, specifies which direction and turning direction the robot is using
|
|
#define DRIVE_BASIC_STOP 0
|
|
#define DRIVE_BASIC_FORWARD 1
|
|
#define DRIVE_BASIC_FRONTLEFT 2
|
|
#define DRIVE_BASIC_FRONTRIGHT 3
|
|
#define DRIVE_BASIC_BACKWARD 4
|
|
#define DRIVE_BASIC_BACKLEFT 5
|
|
#define DRIVE_BASIC_BACKRIGHT 6
|
|
|
|
// Length of the buffer to monitor recent steering encoder positions to calculate speed
|
|
// The buffer will track the last N states of the encoders, and the times at which they were recorded, to determine the steering motors' current speeds
|
|
// This value must always be at least 2, otherwise the code will break due to there being an array with a zero or negative length or a division by zero
|
|
#define ENCODER_BUFFER_ENTRY_COUNT 5
|
|
|
|
// Number of encoder ticks per degree of rotation for the swerve drive steering motors
|
|
#define STEERING_ENCODER_TICKS_PER_DEGREE (1024.0 * 4.0) / 360.0 // TODO check as of 20230927
|
|
|
|
// Maximum speed allowed for the steering motors (out of 127.0)
|
|
#define STEERING_MOTOR_SPEED_LIMIT 127.0 // TODO as of 20230927, lower this if they're spinning too fast for the robot to handle
|
|
|
|
// Start decelerating the steering motors linearly when they're within this many degrees of their target angle
|
|
#define STEERING_SLOW_DELTA 30.0
|
|
|
|
// Claw status
|
|
#define CLAW_UNKNOWN 1 // Position unknown
|
|
#define CLAW_STOPPED 2 // Stopped in current position (somewhere between open and closed)
|
|
#define CLAW_CLOSED 3 // Claw is currently closed
|
|
#define CLAW_OPEN 4 // Claw is currently open
|
|
#define CLAW_MOVING 5 // The claw is currently moving
|
|
|
|
// Claw commands
|
|
#define CLAW_COMMAND_UNSET 0 // No command has been set yet, not doing anything
|
|
// #define CLAW_COMMAND_STOP 1 // Stop immediately, no matter the location
|
|
#define CLAW_COMMAND_CLOSE 2 // Close the claw
|
|
#define CLAW_COMMAND_OPEN 3 // Open the claw
|
|
|
|
// Claw things
|
|
#define CLAW_OPEN_ANGLE 90.0f // Open position of the claw
|
|
#define CLAW_CLOSED_ANGLE -90.0f // Closed position of the claw
|
|
#define CLAW_DEFAULT_ANGLE 0.0f // Default starting claw position
|
|
|
|
// Tilt servo control parameters
|
|
#define TILT_ANGLE_MIN_UPDATE_INTERVAL 0.2f // Update the tilt servo's target angle only after this many seconds have elapsed since the previous angle update
|
|
#define TILT_ANGLE_MIN_UPDATE_LOOPS (TILT_ANGLE_MIN_UPDATE_INTERVAL / LOOP_DELAY_SECONDS) // Previous value expressed as a number of control loops to wait between updates
|
|
#define TILT_ANGLE_UPDATE_DISTANCE 10.0f // Distance in degrees to shift the servo angle by each update
|
|
#define TILT_MAX_ANGLE 90.0f // Maximum angle allowed for the tilt servo
|
|
#define TILT_MIN_ANGLE -90.0f // Minimum angle allowed for the tilt servo
|
|
#define TILT_FLAT_ANGLE 0.0f // Default/flat/starting angle for the tilt servo
|
|
|
|
// Tilt servo commands
|
|
#define TILT_COMMAND_UNSET 0
|
|
#define TILT_COMMAND_UP 1
|
|
#define TILT_COMMAND_DOWN 2
|
|
|
|
#define SerComm Serial1 //Serial port connected to Xbee
|
|
#define DIAMOND_LEFT 0
|
|
#define DIAMOND_DOWN 1
|
|
#define DIAMOND_RIGHT 2
|
|
#define DIAMOND_UP 3
|
|
#define SHOULDER_TOP_LEFT 4
|
|
#define SHOULDER_TOP_RIGHT 5
|
|
#define SHOULDER_BOTTOM_LEFT 6
|
|
#define SHOULDER_BOTTOM_RIGHT 7
|
|
#define SMALL_LEFT 8
|
|
#define SMALL_RIGHT 9
|
|
//10 and 11 are probably the stick buttons
|
|
//but we haven't checked recently
|
|
#define DPAD_UP 12
|
|
#define DPAD_RIGHT 13
|
|
#define DPAD_DOWN 14
|
|
#define DPAD_LEFT 15
|
|
|
|
// pins for motor controller 1 (right)
|
|
#define ALI1 0
|
|
#define AHI1 1
|
|
#define BHI1 2
|
|
#define BLI1 3
|
|
#define DENABLE1 8
|
|
//#define DREADY1 30
|
|
|
|
// and 2 (left)
|
|
#define ALI2 4
|
|
#define AHI2 5
|
|
#define BHI2 6
|
|
#define BLI2 7
|
|
#define DENABLE2 9
|
|
//#define DREADY2 31
|
|
|
|
#define try_enable_right(e,VBATT) try_enable_osmc(e,DENABLE1,VBATT,ALI1,BLI1,AHI1,BHI1)
|
|
#define try_enable_left(e,VBATT) try_enable_osmc(e,DENABLE2,VBATT,ALI2,BLI2,AHI2,BHI2)
|
|
#define drive_right(e,x) drive_osmc(e,DENABLE1,x,0,ALI1,BLI1,AHI1,BHI1)
|
|
#define drive_left(e,x) drive_osmc(e,DENABLE2,x,0,ALI2,BLI2,AHI2,BHI2)
|
|
|
|
#define DEADBAND_HALF_WIDTH 10 // Control input deadband radius
|
|
#define FAILTIME 200 //Failsafe timeout in milliseconds
|
|
#define DEBUGPRINT(x) SerCommDbg.println(x)
|
|
#define SerCommDbg Serial //Serial port for debugging info
|