Skip to content

Commit

Permalink
Rudder implement
Browse files Browse the repository at this point in the history
1. Add Rudder checkbox into Advanced area
2. Recommend using linear force curve for rudder
3. Use Rudder function in your own Risk
4. Add ESPNow comunication
5. I2C is a nogo
6. Displacement-calucation for Rudder is in Plan
  • Loading branch information
tcfshcrw committed Jul 28, 2024
1 parent c3a0ab2 commit f91eccb
Show file tree
Hide file tree
Showing 11 changed files with 815 additions and 21 deletions.
138 changes: 138 additions & 0 deletions ESP32/include/ABSOscillation.h
Original file line number Diff line number Diff line change
Expand Up @@ -430,5 +430,143 @@ class Custom_vibration {
_lastCallTimeMillis = timeNowMillis;


}
};
MovingAverageFilter averagefilter_rudder(30);
MovingAverageFilter averagefilter_rudder_force(50);
class Rudder{
public:
int32_t Center_offset;
int32_t offset_raw;
int32_t offset_filter;
int32_t stepper_range;
int32_t dead_zone_upper;
int32_t dead_zone_lower;
int32_t dead_zone;
int16_t sync_pedal_position;
int16_t current_pedal_position;
float endpos_travel;
float force_range;
float force_offset_raw;
float force_offset_filter;
float force_center_offset;
float position_ratio_sync;
float position_ratio_current;
int debug_count=0;

void offset_calculate(DAP_calculationVariables_st* calcVars_st)
{
dead_zone=20;
Center_offset=calcVars_st->stepperPosRange/2;
dead_zone_upper=Center_offset+dead_zone/2;
dead_zone_lower=Center_offset-dead_zone/2;
sync_pedal_position=calcVars_st->sync_pedal_position;
current_pedal_position=calcVars_st->current_pedal_position;
stepper_range=calcVars_st->stepperPosRange;
force_range=calcVars_st->Force_Range;
force_center_offset=force_range/2;
if(calcVars_st->Rudder_status)
{
if(calcVars_st->pedal_type==1)
{
//pedal as brake
if(sync_pedal_position>dead_zone_upper)
{
offset_raw=-1*(sync_pedal_position-Center_offset)/65536*stepper_range;
}
else
{
offset_raw=0;
}
}
if(calcVars_st->pedal_type==2)
{
//pedal as gas
if(sync_pedal_position>dead_zone_upper)
{
offset_raw=-1*(sync_pedal_position-Center_offset)/65536*stepper_range;
}
else
{
offset_raw=0;
}
}
offset_filter=averagefilter_rudder.process(offset_raw+Center_offset);
}
else
{
offset_filter=0;
}
}
void force_offset_calculate(DAP_calculationVariables_st* calcVars_st)
{
dead_zone=20;
Center_offset=calcVars_st->stepperPosRange/2;
dead_zone_upper=Center_offset+dead_zone/2;
dead_zone_lower=Center_offset-dead_zone/2;
sync_pedal_position=calcVars_st->sync_pedal_position;
current_pedal_position=calcVars_st->current_pedal_position;
stepper_range=calcVars_st->stepperPosRange;
force_range=calcVars_st->Force_Range;
force_center_offset=force_range/2+calcVars_st->Force_Min;
endpos_travel=(float)calcVars_st->stepperPosRange;
//endpos_travel=((float)(calcVars_st->current_pedal_position-calcVars_st->stepperPosMin))/((float)calcVars_st->stepperPosRange);
position_ratio_sync=calcVars_st->Sync_pedal_position_ratio;
position_ratio_current=((float)(current_pedal_position-calcVars_st->stepperPosMin))/endpos_travel;
/*
if(calcVars_st->Rudder_status)
{
if(debug_count>2000)
{
Serial.print(calcVars_st->pedal_type);
Serial.print("--sync_ratio----:");
Serial.print(position_ratio_sync);
Serial.print("---sync position---");
Serial.println(sync_pedal_position);
debug_count=0;
}
else
{
debug_count++;
}
}
*/


float center_deadzone = 0.51;
if(calcVars_st->Rudder_status)
{
if(calcVars_st->pedal_type==1)
{
//pedal as brake
if(position_ratio_sync>center_deadzone)
{
force_offset_raw=(float)(-1*(position_ratio_sync-0.50)*force_range);
//Serial.println(force_offset_raw);
}
else
{
force_offset_raw=0;
}
}
if(calcVars_st->pedal_type==2)
{
//pedal as gas
if(position_ratio_sync>center_deadzone)
{
force_offset_raw=(float)(-1*(position_ratio_sync-0.50)*force_range);
//Serial.println(force_offset_raw);
}
else
{
force_offset_raw=0;
}
}
force_offset_filter=averagefilter_rudder_force.process(force_offset_raw+force_center_offset);
}
else
{
force_offset_filter=0;
}
}
};
9 changes: 8 additions & 1 deletion ESP32/include/DiyActivePedal_types.h
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
#include <stdint.h>

// define the payload revision
#define DAP_VERSION_CONFIG 137
#define DAP_VERSION_CONFIG 138

// define the payload types
#define DAP_PAYLOAD_TYPE_CONFIG 100
Expand Down Expand Up @@ -35,6 +35,7 @@ struct payloadPedalAction {
uint8_t impact_value_u8;
uint8_t Trigger_CV_1;
uint8_t Trigger_CV_2;
uint8_t Rudder_action;
};


Expand Down Expand Up @@ -245,6 +246,12 @@ struct DAP_calculationVariables_st
float Force_Max_default;
float WS_amp;
float WS_freq;
bool Rudder_status;
uint8_t pedal_type;
uint16_t sync_pedal_position;
uint16_t current_pedal_position;
float current_pedal_position_ratio;
float Sync_pedal_position_ratio;

void updateFromConfig(DAP_config_st& config_st);
void updateEndstops(long newMinEndstop, long newMaxEndstop);
Expand Down
95 changes: 95 additions & 0 deletions ESP32/include/ESPNOW_lib.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
#include <WiFi.h>
#include <esp_wifi.h>
#include <Arduino.h>
#include "ESPNowW.h"
//#define ESPNow_debug
uint8_t Clu_mac[] = {0x36, 0x33, 0x33, 0x33, 0x33, 0x32};
uint8_t Gas_mac[] = {0x36, 0x33, 0x33, 0x33, 0x33, 0x33};
uint8_t Brk_mac[] = {0x36, 0x33, 0x33, 0x33, 0x33, 0x34};
uint8_t* Recv_mac;
uint16_t ESPNow_send=0;
uint16_t ESPNow_recieve=0;
//bool MAC_get=false;
bool ESPNOW_status =false;
bool ESPNow_initial_status=false;
bool ESPNow_update= false;
//https://github.com/nickgammon/I2C_Anything/tree/master
struct ESPNow_Send_Struct
{
uint16_t pedal_position;
float pedal_position_ratio;
};
ESPNow_Send_Struct _ESPNow_Recv;
ESPNow_Send_Struct _ESPNow_Send;

void onRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len)
{
/*
if(ESPNOW_status)
{
memcpy(&ESPNow_recieve, data, sizeof(ESPNow_recieve));
ESPNow_update=true;
}
*/
if(ESPNOW_status)
{
memcpy(&_ESPNow_Recv, data, sizeof(_ESPNow_Recv));
ESPNow_update=true;
}

}
void OnSent(const uint8_t *mac_addr, esp_now_send_status_t status)
{

}
void ESPNow_initialize()
{

WiFi.mode(WIFI_MODE_STA);
Serial.println("Initializing Rudder, please wait");
Serial.print("Current MAC Address: ");
Serial.println(WiFi.macAddress());
if(dap_config_st.payLoadPedalConfig_.pedal_type==0)
{
esp_wifi_set_mac(WIFI_IF_STA, &Clu_mac[0]);
}
if(dap_config_st.payLoadPedalConfig_.pedal_type==1)
{
esp_wifi_set_mac(WIFI_IF_STA, &Brk_mac[0]);
}
if(dap_config_st.payLoadPedalConfig_.pedal_type==2)
{
esp_wifi_set_mac(WIFI_IF_STA, &Gas_mac[0]);
}
delay(300);
Serial.print("Modified MAC Address: ");
Serial.println(WiFi.macAddress());
ESPNow.init();
Serial.println("wait 10s for ESPNOW initialized");
delay(10000);

if(dap_config_st.payLoadPedalConfig_.pedal_type==1)
{
Recv_mac=Gas_mac;
}

if(dap_config_st.payLoadPedalConfig_.pedal_type==2)
{
Recv_mac=Brk_mac;
}
if(ESPNow.add_peer(Recv_mac)== ESP_OK)
{
ESPNOW_status=true;
Serial.println("Sucess to add peer");
}
else
{
ESPNOW_status=false;
Serial.println("Fail to add peer");
}
ESPNow.reg_recv_cb(onRecv);
ESPNow.reg_send_cb(OnSent);
ESPNow_initial_status=true;
Serial.println("Rudder Initialized");

}
75 changes: 75 additions & 0 deletions ESP32/include/I2CSync.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@
#include <Wire.h>
#include <Arduino.h>
#include "Main.h"

uint16_t I2C_Read;
#define I2C_rate 100000
#define I2C_debug_out
//TwoWire I2C_sync(0);
bool I2C_sync_status=false;
bool I2C_data_read=false;
uint16_t I2C_send;
uint32_t iii = 0;

//https://github.com/nickgammon/I2C_Anything/tree/master
template <typename T> unsigned int I2C_writeAnything (const T& value)
{
Wire.write((byte *) &value, sizeof (value));
return sizeof (value);
} // end of I2C_writeAnything

template <typename T> unsigned int I2C_readAnything(T& value)
{
byte * p = (byte*) &value;
unsigned int i;
for (i = 0; i < sizeof value; i++)
*p++ = Wire.read();
return i;
} // end of I2C_readAnything

void SynconReceive(int len)
{
/*
Serial.printf("onReceive[%d]: ", len);
while (I2C_sync.available()) {
Serial.write(I2C_sync.read());
}
Serial.println();
*/
if(len>0)
{
I2C_readAnything(I2C_Read);
I2C_data_read=true;
}


//Serial.println();

}
void SynconRequest()
{
/*
I2C_sync.print(iii++);
I2C_sync.print(" Packets.");
Serial.println("onRequest");
Serial.println();
*/
I2C_writeAnything(I2C_send);
//Wire.write(I2C_send);
}

void I2C_initialize()
{
Wire.begin(I2C_SDA,I2C_SCL,I2C_rate);
delay(3000);
Serial.println("Sync as Master");
}

void I2C_initialize_slave()
{
Wire.onReceive(SynconReceive);
Wire.onRequest(SynconRequest);
Wire.begin((uint8_t)I2C_slave_address,I2C_SDA,I2C_SCL,I2C_rate);
//I2C_sync.begin(I2C_SDA,I2C_SCL,(uint8_t)I2C_slave_address);
Serial.println("Sync as Slave");
}
9 changes: 7 additions & 2 deletions ESP32/include/Main.h
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,10 @@
#define dirPinStepper 22
#define stepPinStepper 23
//analog output pin
#define D_O 25
#define D_O 25
//I2Cpins
#define I2C_SDA 32
#define I2C_SCL 33

// endstop pins
#define minPin 12
Expand All @@ -145,7 +148,9 @@
#define ISV57_RXPIN 26 // 16

#define Using_analog_output

//#define Using_I2C_Sync
#define ESPNOW_Enable
#define I2C_slave_address 0x15
#define BLUETOOTH_GAMEPAD
//#define USB_JOYSTICK
#define SERIAL_COOMUNICATION_TASK_DELAY_IN_MS 1
Expand Down
1 change: 1 addition & 0 deletions ESP32/src/DiyActivePedal_types.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -181,6 +181,7 @@ void DAP_calculationVariables_st::updateFromConfig(DAP_config_st& config_st) {
Force_Max = ((float)config_st.payLoadPedalConfig_.maxForce);
Force_Range = Force_Max - Force_Min;
Force_Max_default=((float)config_st.payLoadPedalConfig_.maxForce);
pedal_type=config_st.payLoadPedalConfig_.pedal_type;
}

void DAP_calculationVariables_st::dynamic_update()
Expand Down
Loading

0 comments on commit f91eccb

Please sign in to comment.