109 lines
2.7 KiB
C++
109 lines
2.7 KiB
C++
#include "Dynamixel_Servo.h"
|
|
|
|
#define HALF_DUPLEX_DIRECTION_PIN 6
|
|
#define SERVO_TIMEOUT 200 //milliseconds
|
|
|
|
#define FORWARD_DIRECTION_PIN 7
|
|
#define REVERSE_DIRECTION_PIN 8
|
|
|
|
#define STEERING_INPUT 14
|
|
#define THROTTLE_INPUT 15
|
|
|
|
#define THROTTLE_A 3
|
|
#define THROTTLE_B 5
|
|
|
|
// #define LOWEST_PULSE_THROTTLE 17850
|
|
// #define HIGHEST_PULSE_THROTTLE 18700
|
|
|
|
#define LOWEST_PULSE_THROTTLE 17950
|
|
#define HIGHEST_PULSE_THROTTLE 18900
|
|
|
|
#define LOWEST_PULSE_STEERING 17780
|
|
#define HIGHEST_PULSE_STEERING 18790
|
|
|
|
|
|
static float CHAN_A_MIN_VOLTAGE = 1.37 - 0.11;
|
|
static float CHAN_A_MAX_VOLTAGE = (3.82 - 0.28)*0.50; // NOTE: Go lower bc we don't wanna go supersonic during the demo
|
|
static float CHAN_B_MIN_VOLTAGE = 0.68 - 0.15;
|
|
static float CHAN_B_MAX_VOLTAGE = (3.18 - 0.37)*0.50; // NOTE: Go lower bc we don't wanna go supersonic during the demo
|
|
|
|
int32_t throttle_in;
|
|
int32_t steering_in;
|
|
int32_t throttle_in_mapped;
|
|
int32_t steering_in_mapped;
|
|
|
|
int throttle_speed;
|
|
int steering_amount;
|
|
|
|
int mapped_throttle_chan_a;
|
|
int mapped_throttle_chan_b;
|
|
|
|
struct channel_outputs {
|
|
double chan_a;
|
|
double chan_b;
|
|
};
|
|
|
|
float mapping(float in, float in_min, float in_max, float out_min, float out_max){
|
|
float mapped = (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min;
|
|
return mapped;
|
|
}
|
|
|
|
|
|
void setup() {
|
|
pinMode(STEERING_INPUT, INPUT_PULLUP);
|
|
pinMode(THROTTLE_INPUT, INPUT_PULLUP);
|
|
pinMode(THROTTLE_A, OUTPUT);
|
|
pinMode(THROTTLE_B, OUTPUT);
|
|
pinMode(FORWARD_DIRECTION_PIN, OUTPUT);
|
|
pinMode(REVERSE_DIRECTION_PIN, OUTPUT);
|
|
digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
|
|
digitalWrite(REVERSE_DIRECTION_PIN, HIGH);
|
|
|
|
servo_init(&Serial1, HALF_DUPLEX_DIRECTION_PIN, SERVO_DEFAULT_BAUD);
|
|
|
|
Serial.begin(9600);
|
|
delay(2000);
|
|
Serial.println("Starting...");
|
|
}
|
|
|
|
|
|
void steering_zero() {
|
|
servo_error_t error;
|
|
int steering_angle_deg = 0;
|
|
error = servo_set(SERVO_DEFAULT_ID, SERVO_REGISTER_GOAL_ANGLE, steering_angle_deg, SERVO_TIMEOUT);
|
|
Serial.print(" STEERING ZERO ");
|
|
}
|
|
|
|
void set_steering(float angle_deg){
|
|
servo_error_t error;
|
|
float angle_rad = angle_deg * 1000. / 57296.;
|
|
error = servo_set(SERVO_DEFAULT_ID, SERVO_REGISTER_GOAL_ANGLE, angle_rad, SERVO_TIMEOUT);
|
|
if (! (error & SERVO_NO_ERROR)){
|
|
Serial.print("SERVO ERROR: ");
|
|
Serial.println(error);
|
|
}
|
|
}
|
|
|
|
|
|
void steering(int32_t steering_pwm) {
|
|
if (steering_pwm < 17000) {
|
|
steering_zero();
|
|
} else {
|
|
steering_in_mapped = map(steering_pwm, LOWEST_PULSE_STEERING, HIGHEST_PULSE_STEERING, 0, 1023);
|
|
set_steering(steering_in_mapped);
|
|
}
|
|
}
|
|
|
|
|
|
void loop() {
|
|
steering_in = pulseIn(STEERING_INPUT, LOW);
|
|
Serial.print(" SPIN: ");
|
|
Serial.print(steering_in);
|
|
// steering(steering_in);
|
|
set_steering(512);
|
|
delay(500);
|
|
set_steering(0);
|
|
delay(500);
|
|
Serial.println();
|
|
}
|