282 lines
7.6 KiB
C++
282 lines
7.6 KiB
C++
#include <Adafruit_MCP4728.h>
|
|
#include <Wire.h>
|
|
|
|
Adafruit_MCP4728 mcp;
|
|
|
|
#define MAX_V 5.2
|
|
|
|
#define FORK_VERT_INPUT 14
|
|
#define FORK_HORIZ_INPUT 15
|
|
|
|
#define LOWEST_PULSE 17770
|
|
#define HIGHEST_PULSE 18790
|
|
// Recorded: L: 17791 H: 18786
|
|
// #define PULSE_DELTA = 1100; // this doesn't change
|
|
// Measured voltages: V: LA:1.04 MA:2.40 HA:3.76 LB:0.80 MB:2.16 HB:3.52
|
|
// Measured voltages: H: LA:1.04 MA:2.40 HA:3.76 LB:0.96 MB:2.32 HB:3.60
|
|
/*
|
|
CHAN_A IS VERT A
|
|
CHAN_B IS VERT B
|
|
CHAN_C IS HORIZ A
|
|
CHAN_D IS HORIZ B
|
|
*/
|
|
|
|
static float CHAN_A_V_MIN_VOLTAGE = 1.04 + 0.0;
|
|
static float CHAN_A_V_MAX_VOLTAGE = 3.76 + 0.0;
|
|
static float CHAN_B_V_MIN_VOLTAGE = 0.80 - 0.0;
|
|
static float CHAN_B_V_MAX_VOLTAGE = 3.52 - 0.0;
|
|
|
|
static float CHAN_A_H_MIN_VOLTAGE = 1.04 + 0.0;
|
|
static float CHAN_A_H_MAX_VOLTAGE = 3.76 + 0.0;
|
|
static float CHAN_B_H_MIN_VOLTAGE = 0.96 + 0.0;
|
|
static float CHAN_B_H_MAX_VOLTAGE = 3.60 + 0.0;
|
|
|
|
static float STOP_VOLTAGE_V_A = 2.40 + 0.08;
|
|
static float STOP_VOLTAGE_V_B = 2.16 + 0.00;
|
|
|
|
static float STOP_VOLTAGE_H_A = 2.40 + 0.0;
|
|
static float STOP_VOLTAGE_H_B = 2.32 + 0.0;
|
|
|
|
int32_t fork_vert_in;
|
|
int32_t fork_horiz_in;
|
|
int32_t fork_vert_in_mapped;
|
|
int32_t fork_horiz_in_mapped;
|
|
|
|
int fork_vert_speed;
|
|
int fork_horiz_speed;
|
|
|
|
int mapped_fork_vert_chan_a;
|
|
int mapped_fork_vert_chan_b;
|
|
int mapped_fork_horiz_chan_a;
|
|
int mapped_fork_horiz_chan_b;
|
|
|
|
struct channel_outputs {
|
|
double chan_a;
|
|
double chan_b;
|
|
};
|
|
|
|
//helper functions
|
|
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;
|
|
}
|
|
|
|
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 adjusted_v;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
//helper functions
|
|
|
|
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 set_voltage_c(float v){
|
|
int v_int = process_voltage(v);
|
|
mcp.setChannelValue(MCP4728_CHANNEL_C, v_int);
|
|
}
|
|
void set_voltage_d(float v){
|
|
int v_int = process_voltage(v);
|
|
mcp.setChannelValue(MCP4728_CHANNEL_D, v_int);
|
|
}
|
|
|
|
void stop_fork_vert() {
|
|
set_voltage_a(low_voltage_compensator(STOP_VOLTAGE_V_A));
|
|
set_voltage_b(low_voltage_compensator(STOP_VOLTAGE_V_B));
|
|
Serial.print(" VERT STOPPED ");
|
|
}
|
|
|
|
void stop_fork_horiz() {
|
|
set_voltage_c(low_voltage_compensator(STOP_VOLTAGE_H_A));
|
|
set_voltage_d(low_voltage_compensator(STOP_VOLTAGE_H_B));
|
|
Serial.print(" HORIZ STOPPED ");
|
|
}
|
|
|
|
void fork_up(int speed){
|
|
channel_outputs pwm_out = pwm_to_outputs_v_low(speed);
|
|
set_voltage_a(pwm_out.chan_a);
|
|
set_voltage_b(pwm_out.chan_b);
|
|
}
|
|
|
|
void fork_down(int speed){
|
|
channel_outputs pwm_out = pwm_to_outputs_v_high(speed);
|
|
set_voltage_a(pwm_out.chan_a);
|
|
set_voltage_b(pwm_out.chan_b);
|
|
}
|
|
|
|
void fork_left(int speed){
|
|
channel_outputs pwm_out = pwm_to_outputs_h_high(speed);
|
|
set_voltage_c(pwm_out.chan_a);
|
|
set_voltage_d(pwm_out.chan_b);
|
|
}
|
|
|
|
void fork_right(int speed){
|
|
channel_outputs pwm_out = pwm_to_outputs_h_low(speed);
|
|
set_voltage_c(pwm_out.chan_a);
|
|
set_voltage_d(pwm_out.chan_b);
|
|
}
|
|
|
|
channel_outputs pwm_to_outputs_v_high(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., STOP_VOLTAGE_V_A, CHAN_A_V_MAX_VOLTAGE);
|
|
out_voltage_b = mapping(255 - pwm, 0, 255, CHAN_B_V_MIN_VOLTAGE, STOP_VOLTAGE_V_B);
|
|
Serial.print(" PWM: ");
|
|
Serial.print(pwm);
|
|
Serial.print(" HV_A: ");
|
|
Serial.print(out_voltage_a);
|
|
Serial.print(" HV_B: ");
|
|
Serial.print(out_voltage_b);
|
|
pwm_out.chan_a = out_voltage_a;
|
|
pwm_out.chan_b = out_voltage_b;
|
|
return (pwm_out);
|
|
}
|
|
|
|
channel_outputs pwm_to_outputs_v_low(int pwm){
|
|
channel_outputs pwm_out;
|
|
float out_voltage_a;
|
|
float out_voltage_b;
|
|
int out_pwm_a;
|
|
int out_pwm_b;
|
|
out_voltage_a = mapping(255 - pwm, 0., 255., CHAN_A_V_MIN_VOLTAGE, STOP_VOLTAGE_V_A);
|
|
out_voltage_b = mapping(pwm, 0., 255., STOP_VOLTAGE_V_B, CHAN_B_V_MAX_VOLTAGE);
|
|
Serial.print(" PWM: ");
|
|
Serial.print(pwm);
|
|
Serial.print(" LV_A: ");
|
|
Serial.print(out_voltage_a);
|
|
Serial.print(" LV_B: ");
|
|
Serial.print(out_voltage_b);
|
|
pwm_out.chan_a = out_voltage_a;
|
|
pwm_out.chan_b = out_voltage_b;
|
|
return (pwm_out);
|
|
}
|
|
|
|
channel_outputs pwm_to_outputs_h_low(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., STOP_VOLTAGE_H_A, CHAN_A_H_MAX_VOLTAGE);
|
|
out_voltage_b = mapping(255 - pwm, 0, 255, CHAN_B_H_MIN_VOLTAGE, STOP_VOLTAGE_H_B);
|
|
Serial.print(" PWM: ");
|
|
Serial.print(pwm);
|
|
Serial.print(" HV_A: ");
|
|
Serial.print(out_voltage_a);
|
|
Serial.print(" HV_B: ");
|
|
Serial.print(out_voltage_b);
|
|
pwm_out.chan_a = out_voltage_a;
|
|
pwm_out.chan_b = out_voltage_b;
|
|
return (pwm_out);
|
|
}
|
|
|
|
channel_outputs pwm_to_outputs_h_high(int pwm){
|
|
channel_outputs pwm_out;
|
|
float out_voltage_a;
|
|
float out_voltage_b;
|
|
int out_pwm_a;
|
|
int out_pwm_b;
|
|
out_voltage_a = mapping(255 - pwm, 0., 255., CHAN_A_H_MIN_VOLTAGE, STOP_VOLTAGE_H_A);
|
|
out_voltage_b = mapping(pwm, 0., 255., STOP_VOLTAGE_H_B, CHAN_B_H_MAX_VOLTAGE);
|
|
Serial.print(" PWM: ");
|
|
Serial.print(pwm);
|
|
Serial.print(" LV_A: ");
|
|
Serial.print(out_voltage_a);
|
|
Serial.print(" LV_B: ");
|
|
Serial.print(out_voltage_b);
|
|
pwm_out.chan_a = out_voltage_a;
|
|
pwm_out.chan_b = out_voltage_b;
|
|
return (pwm_out);
|
|
}
|
|
|
|
void fork_vert(int32_t vert_pwm) {
|
|
if (vert_pwm < 17000) {
|
|
stop_fork_vert();
|
|
} else {
|
|
fork_vert_in_mapped = map(vert_pwm, LOWEST_PULSE, HIGHEST_PULSE, 0, 1000);
|
|
if (fork_vert_in_mapped >= 400 && fork_vert_in_mapped <= 600) {stop_fork_vert();}
|
|
else if (fork_vert_in_mapped > 600) {
|
|
fork_vert_speed = map(fork_vert_in_mapped, 600, 1000, 0, 255);
|
|
fork_up(fork_vert_speed);
|
|
} else if (fork_vert_in_mapped < 400) {
|
|
fork_vert_speed = map(fork_vert_in_mapped, 0, 400, 0, 255);
|
|
fork_vert_speed = 255 - fork_vert_speed;
|
|
fork_down(fork_vert_speed);
|
|
}
|
|
}
|
|
}
|
|
|
|
void fork_horiz(int32_t horiz_pwm) {
|
|
if (horiz_pwm < 17000) {
|
|
stop_fork_horiz();
|
|
} else {
|
|
fork_horiz_in_mapped = map(horiz_pwm, LOWEST_PULSE, HIGHEST_PULSE, 0, 1000);
|
|
if (fork_horiz_in_mapped >= 400 && fork_horiz_in_mapped <= 600) {stop_fork_horiz();}
|
|
else if (fork_horiz_in_mapped > 600) {
|
|
fork_horiz_speed = map(fork_horiz_in_mapped, 600, 1000, 0, 255);
|
|
fork_right(fork_horiz_speed);
|
|
} else if (fork_horiz_in_mapped < 400) {
|
|
fork_horiz_speed = map(fork_horiz_in_mapped, 0, 400, 0, 255);
|
|
fork_horiz_speed = 255 - fork_horiz_speed;
|
|
fork_left(fork_horiz_speed);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void setup() {
|
|
pinMode(FORK_VERT_INPUT, INPUT_PULLUP);
|
|
pinMode(FORK_HORIZ_INPUT, INPUT_PULLUP);
|
|
Serial.begin(9600);
|
|
delay(2000);
|
|
Serial.println("Starting...");
|
|
if (!mcp.begin()) {
|
|
Serial.println("Failed to find MCP4728 chip");
|
|
while (1) {
|
|
delay(10);
|
|
}
|
|
}
|
|
Serial.println("Started.");
|
|
}
|
|
|
|
void loop() {
|
|
fork_vert_in = pulseIn(FORK_VERT_INPUT, LOW);
|
|
Serial.print(" VPIN: ");
|
|
Serial.print(fork_vert_in);
|
|
fork_vert(fork_vert_in);
|
|
Serial.print(" || ");
|
|
fork_horiz_in = pulseIn(FORK_HORIZ_INPUT, LOW);
|
|
Serial.print(" HPIN: ");
|
|
Serial.print(fork_horiz_in);
|
|
fork_horiz(fork_horiz_in);
|
|
Serial.println();
|
|
delay(1);
|
|
}
|