#pragma once #include "packet.h" #include #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