287 lines
7.2 KiB
C++
287 lines
7.2 KiB
C++
#include <Dynamixel2Arduino.h>
|
|
#include <Adafruit_MCP4728.h>
|
|
#include <Wire.h>
|
|
|
|
#define DXL_SERIAL Serial1
|
|
#define DEBUG_SERIAL Serial
|
|
const uint8_t DXL_DIR_PIN = 4; // DYNAMIXEL Shield DIR PIN
|
|
|
|
#define MAX_V 5.2
|
|
#define SPEED_MULTIPLIER 0.8
|
|
|
|
const uint8_t DXL_ID = 1;
|
|
const float DXL_PROTOCOL_VERSION = 1.0;
|
|
|
|
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
|
|
using namespace ControlTableItem;
|
|
|
|
Adafruit_MCP4728 mcp;
|
|
|
|
long zero_offset = 0.0;
|
|
|
|
//TOP THROT: 18792 | BOT THROT: 17991 | TOP STEER: 19092 | BOT STEER: 17691
|
|
|
|
|
|
#define FORWARD_DIRECTION_PIN 7
|
|
#define REVERSE_DIRECTION_PIN 8
|
|
|
|
#define STEERING_INPUT 14
|
|
#define THROTTLE_INPUT 15
|
|
|
|
#define TRIM_RIGHT 16
|
|
#define TRIM_LEFT 10 //TODO: change pins
|
|
#define TRIM_DELTA 100 //TODO: tune this
|
|
|
|
|
|
#define LOWEST_PULSE_THROTTLE 17970
|
|
#define HIGHEST_PULSE_THROTTLE 18800
|
|
|
|
// #define LOWEST_PULSE_STEERING 17650
|
|
// #define HIGHEST_PULSE_STEERING 19150
|
|
|
|
#define LOWEST_PULSE_STEERING 17600
|
|
#define HIGHEST_PULSE_STEERING 19200
|
|
|
|
|
|
static float CHAN_A_MIN_VOLTAGE = 1.37 + 0.0;
|
|
static float CHAN_A_MAX_VOLTAGE = (3.82 + 0.0) * SPEED_MULTIPLIER; // NOTE: Go lower bc we don't wanna go supersonic during the demo
|
|
static float CHAN_B_MIN_VOLTAGE = 0.68 + 0.0;
|
|
static float CHAN_B_MAX_VOLTAGE = (3.18 + 0.0) * SPEED_MULTIPLIER; // 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;
|
|
long 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 init_servo(){
|
|
dxl.begin(57600);
|
|
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
|
|
dxl.ping(DXL_ID);
|
|
dxl.torqueOff(DXL_ID);
|
|
dxl.setOperatingMode(DXL_ID, OP_EXTENDED_POSITION);
|
|
dxl.writeControlTableItem(RESOLUTION_DIVIDER, DXL_ID, 1);
|
|
dxl.torqueOn(DXL_ID);
|
|
dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 0);
|
|
delay(500);
|
|
}
|
|
|
|
int convert_to_digital(float v){
|
|
int v_out;
|
|
v_out = (v/5.0) * 4095;
|
|
return v_out;
|
|
}
|
|
int minmax(int v){
|
|
if (v > 4095) {v = 4095;}
|
|
if (v < 0) {v = 0;}
|
|
return v;
|
|
}
|
|
|
|
float low_voltage_compensator(float v){
|
|
float v_offset = 5.0 - MAX_V;
|
|
float v_proportional = v/5.0;
|
|
float adjusted_v = v + (v_proportional * v_offset);
|
|
// return v;
|
|
return adjusted_v;
|
|
}
|
|
|
|
int process_voltage(float v){
|
|
float v_adj = low_voltage_compensator(v);
|
|
int v_int = convert_to_digital(v_adj);
|
|
v_int = minmax(v_int);
|
|
return v_int;
|
|
}
|
|
|
|
void set_voltage_a(float v){
|
|
int v_int = process_voltage(v);
|
|
mcp.setChannelValue(MCP4728_CHANNEL_A, v_int);
|
|
}
|
|
void set_voltage_b(float v){
|
|
int v_int = process_voltage(v);
|
|
mcp.setChannelValue(MCP4728_CHANNEL_B, v_int);
|
|
}
|
|
|
|
void increment_trim(int amount){
|
|
zero_offset = zero_offset + amount;
|
|
}
|
|
|
|
void decrement_trim(int amount){
|
|
zero_offset = zero_offset - amount;
|
|
}
|
|
|
|
void stop_throttle() {
|
|
set_voltage_a(CHAN_A_MIN_VOLTAGE);
|
|
set_voltage_b(CHAN_B_MIN_VOLTAGE);
|
|
Serial.print(" THROTTLE STOPPED ");
|
|
}
|
|
|
|
void steering_zero() {
|
|
dxl.setGoalPosition(DXL_ID, 0+zero_offset);
|
|
}
|
|
|
|
void set_throttle(int speed){
|
|
channel_outputs pwm_out = pwm_to_outputs(speed);
|
|
set_voltage_a(pwm_out.chan_a);
|
|
set_voltage_b(pwm_out.chan_b);
|
|
}
|
|
|
|
void go_forward(int speed){
|
|
set_forward();
|
|
set_throttle(speed);
|
|
}
|
|
|
|
void go_back(int speed){
|
|
set_reverse();
|
|
set_throttle(speed);
|
|
}
|
|
|
|
void stop(){
|
|
stop_throttle();
|
|
set_neutral();
|
|
}
|
|
|
|
void set_forward(){
|
|
digitalWrite(REVERSE_DIRECTION_PIN, HIGH);
|
|
digitalWrite(FORWARD_DIRECTION_PIN, LOW);
|
|
}
|
|
|
|
void set_reverse(){
|
|
digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
|
|
digitalWrite(REVERSE_DIRECTION_PIN, LOW);
|
|
}
|
|
|
|
void set_neutral(){
|
|
digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
|
|
digitalWrite(REVERSE_DIRECTION_PIN, HIGH);
|
|
}
|
|
|
|
void set_steering(long steering_raw) {
|
|
dxl.setGoalPosition(DXL_ID, steering_raw+zero_offset);
|
|
}
|
|
|
|
channel_outputs pwm_to_outputs(int pwm){
|
|
channel_outputs pwm_out;
|
|
float out_voltage_a = 0.;
|
|
float out_voltage_b = 0.;
|
|
int out_pwm_a;
|
|
int out_pwm_b;
|
|
out_voltage_a = mapping(pwm, 0., 255., CHAN_A_MIN_VOLTAGE, CHAN_A_MAX_VOLTAGE);
|
|
out_voltage_b = mapping(pwm, 0, 255, CHAN_B_MIN_VOLTAGE, CHAN_B_MAX_VOLTAGE);
|
|
Serial.print(" PWM: ");
|
|
Serial.print(pwm);
|
|
Serial.print(" T_A: ");
|
|
Serial.print(out_voltage_a);
|
|
Serial.print(" T_B: ");
|
|
Serial.print(out_voltage_b);
|
|
pwm_out.chan_a = out_voltage_a;
|
|
pwm_out.chan_b = out_voltage_b;
|
|
return (pwm_out);
|
|
}
|
|
|
|
void throttle(int32_t throttle_pwm) {
|
|
if (throttle_pwm < 1600) {
|
|
stop();
|
|
} else {
|
|
throttle_in_mapped = map(throttle_pwm, LOWEST_PULSE_THROTTLE, HIGHEST_PULSE_THROTTLE, 0, 1000);
|
|
if (throttle_in_mapped >= 400 && throttle_in_mapped <= 600) {stop();}
|
|
else if (throttle_in_mapped > 600) {
|
|
throttle_speed = map(throttle_in_mapped, 600, 1000, 0, 255);
|
|
go_forward(throttle_speed);
|
|
} else if (throttle_in_mapped < 400) {
|
|
throttle_speed = map(throttle_in_mapped, 0, 400, 0, 255);
|
|
throttle_speed = 255 - throttle_speed;
|
|
go_back(throttle_speed);
|
|
}
|
|
}
|
|
}
|
|
|
|
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, -7796, 7796);
|
|
Serial.print(" GT: ");
|
|
Serial.print(steering_in_mapped);
|
|
if (steering_in_mapped >= -200 && steering_in_mapped <= 200) {steering_zero();}
|
|
else {
|
|
set_steering(steering_in_mapped);
|
|
}
|
|
}
|
|
}
|
|
|
|
void check_trim_pins(){
|
|
if (digitalRead(TRIM_RIGHT) == LOW) {increment_trim(TRIM_DELTA); delay(100);}
|
|
if (digitalRead(TRIM_LEFT) == LOW) {decrement_trim(TRIM_DELTA); delay(100);}
|
|
}
|
|
|
|
|
|
void setup() {
|
|
delay(1000); // Give time for Dynamixel to start on power-up
|
|
pinMode(STEERING_INPUT, INPUT_PULLUP);
|
|
pinMode(THROTTLE_INPUT, INPUT_PULLUP);
|
|
pinMode(FORWARD_DIRECTION_PIN, OUTPUT);
|
|
pinMode(REVERSE_DIRECTION_PIN, OUTPUT);
|
|
pinMode(TRIM_RIGHT, INPUT_PULLUP);
|
|
pinMode(TRIM_LEFT, INPUT_PULLUP);
|
|
digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
|
|
digitalWrite(REVERSE_DIRECTION_PIN, HIGH);
|
|
|
|
init_servo();
|
|
|
|
DEBUG_SERIAL.begin(9600);
|
|
delay(500);
|
|
Serial.println("Starting...");
|
|
|
|
long current_position = dxl.getPresentPosition(DXL_ID);
|
|
zero_offset = current_position;
|
|
Serial.print("Zero_offset: ");
|
|
Serial.println(zero_offset);
|
|
delay(100);
|
|
|
|
Serial.print("Going to Zero (we should not move...): ");
|
|
Serial.print(zero_offset);
|
|
Serial.println(" Please wait for 2000ms...");
|
|
dxl.setGoalPosition(DXL_ID, zero_offset); delay(2000);
|
|
|
|
current_position = dxl.getPresentPosition(DXL_ID);
|
|
Serial.print("now at: ");
|
|
Serial.println(current_position);
|
|
|
|
if (!mcp.begin()) {
|
|
Serial.println("Failed to find MCP4728 chip");
|
|
while (1) {
|
|
delay(10);
|
|
}
|
|
}
|
|
Serial.println("Started.");
|
|
}
|
|
|
|
void loop() {
|
|
throttle_in = pulseIn(THROTTLE_INPUT, LOW);
|
|
Serial.print(" TPIN: ");
|
|
Serial.print(throttle_in);
|
|
throttle(throttle_in);
|
|
Serial.print(" || ");
|
|
steering_in = pulseIn(STEERING_INPUT, LOW);
|
|
Serial.print(" SPIN: ");
|
|
Serial.print(steering_in);
|
|
steering(steering_in);
|
|
Serial.println();
|
|
check_trim_pins();
|
|
}
|