interfisch
/
bobbycar
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Compare commits

base: interfisch:7516b76d2a366e0d5b1044bf5efee317bfc8c40c
Branches Tags
interfisch:master
interfisch:20240628
interfisch:20420613
..
compare: interfisch:76e2aad1c86b973be335d01acda25afe40b2f39b
Branches Tags
interfisch:master
interfisch:20240628
interfisch:20420613

No commits in common. "7516b76d2a366e0d5b1044bf5efee317bfc8c40c" and "76e2aad1c86b973be335d01acda25afe40b2f39b" have entirely different histories.
7516b76d2a ... 76e2aad1c8

9 changed files with 52 additions and 275 deletions
Show Stats Expand all files Collapse all files

3
.gitmodules vendored
View File

@ -1,6 +1,3 @@
[submodule "hoverboard-firmware-hack-foc-serial-esc"] [submodule "hoverboard-firmware-hack-foc-serial-esc"]
path = hoverboard-firmware-hack-foc-serial-esc path = hoverboard-firmware-hack-foc-serial-esc
url = https://repos.ctdo.de/interfisch/hoverboard-firmware-hack-foc-serial-esc url = https://repos.ctdo.de/interfisch/hoverboard-firmware-hack-foc-serial-esc
[submodule "controller_teensy/lib/hoverboard-esc-serial-comm"]
path = controller_teensy/lib/hoverboard-esc-serial-comm
url = git@git.ctdo.de:interfisch/hoverboard-esc-serial-comm.git

4
controller_teensy/include/comms.h
View File

@ -106,7 +106,7 @@ void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _lo
} }
/*
void writeLogInfo(HardwareSerial &SerialRef) { //first line of file void writeLogInfo(HardwareSerial &SerialRef) { //first line of file
SerialRef.print("#TIMESTAMP:"); SerialRef.print("#TIMESTAMP:");
SerialRef.println(now()); SerialRef.println(now());
@ -156,7 +156,7 @@ void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)
{ {
SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println(); SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
} }
*/
#endif #endif

16
controller_teensy/include/definitions.h
View File

@ -11,7 +11,7 @@
bool log_update=true; bool log_update=true;
unsigned long last_log_send=0; unsigned long last_log_send=0;
//#define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial #define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial
#define LOGMININTERVAL 20 //minimum interval (ms) to send logs #define LOGMININTERVAL 20 //minimum interval (ms) to send logs
#define LOGMAXINTERVAL 10000 //maximum time (ms) after which data is send #define LOGMAXINTERVAL 10000 //maximum time (ms) after which data is send
@ -19,10 +19,10 @@ unsigned long last_log_send=0;
bool log_header_written = false; bool log_header_written = false;
//#define FEEDBACKRECEIVETIMEOUT 500 #define FEEDBACKRECEIVETIMEOUT 500
//bool controllerFront_connected=false; bool controllerFront_connected=false;
//bool controllerRear_connected=false; bool controllerRear_connected=false;
bool controllers_connected=false; bool controllers_connected=false;
#define PIN_THROTTLE A7 #define PIN_THROTTLE A7
@ -90,13 +90,14 @@ unsigned long last_notidle=0; //not rolling to fast, no pedal pressed
#define PIN_LATCH_ENABLE A6 #define PIN_LATCH_ENABLE A6
#define PIN_MODE_SWITCH 3
#define PIN_MODE_LEDG 4 #define PIN_MODE_LEDG 4
#define PIN_MODE_LEDR 5 #define PIN_MODE_LEDR 5
//unsigned long last_send = 0; unsigned long last_send = 0;
//unsigned long last_receive = 0; unsigned long last_receive = 0;
float filtered_currentAll=0; float filtered_currentAll=0;
@ -122,7 +123,4 @@ bool armed = false; //cmd output values forced to 0 if false
#define DISPLAYUPDATEPERIOD 100 #define DISPLAYUPDATEPERIOD 100
#endif #endif

16
controller_teensy/include/display.h
View File

@ -13,30 +13,24 @@
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
void display_init(); void display_init();
void display_update(); void display_test();
void display_init(){ void display_init(){
if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) { if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
Serial.println(F("SSD1306 allocation failed")); Serial.println(F("SSD1306 allocation failed"));
} }
display.clearDisplay(); display.clearDisplay();
display.setTextSize(1); // Normal 1:1 pixel scale
display.setTextColor(SSD1306_WHITE); // Draw white text
display.setCursor(0,0); // Start at top-left corner
display.display(); display.display();
} }
void display_update(){ void display_test(){
display.clearDisplay(); display.clearDisplay();
display.setTextSize(1); // Normal 1:1 pixel scale display.setTextSize(1); // Normal 1:1 pixel scale
display.setTextColor(SSD1306_WHITE); // Draw white text display.setTextColor(SSD1306_WHITE); // Draw white text
display.setCursor(0,0); // Start at top-left corner display.setCursor(0,0); // Start at top-left corner
display.print(F("Speed : ")); display.println(meanSpeedms); display.println(F("Hello Welt"));
display.print(F("Throttle: ")); display.println(throttle_pos); display.println(millis());
display.print(F("Brake : ")); display.println(brake_pos); display.println(now());
display.print(F("Current : ")); display.println(filtered_currentAll);
display.display(); display.display();
} }

2
controller_teensy/include/helpfunctions.h
View File

@ -1,7 +1,7 @@
#ifndef _HELPFUNCTIONS_H_ #ifndef _HELPFUNCTIONS_H_
#define _HELPFUNCTIONS_H_ #define _HELPFUNCTIONS_H_
//#include "definitions.h" #include "definitions.h"
int sort_desc(const void *cmp1, const void *cmp2); int sort_desc(const void *cmp1, const void *cmp2);
float filterMedian(int16_t* values); float filterMedian(int16_t* values);

115
controller_teensy/include/logging.h
View File

@ -1,115 +0,0 @@
#ifndef _LOGGING_H_
#define _LOGGING_H_
// SD Card Logging
#include <SPI.h> //SCK=13, MISO=12, MOSI=11
#include <SD.h> //Format sd cart with FAT or FAT16
#define SDCHIPSELECT 15
boolean datalogging=true;
String datalogging_filename="UNKNOWN.txt";
void initLogging();
void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear);
void writeLogComment(unsigned long time, String msg);
void initLogging() {
Serial.print("Initializing SD card...");
// see if the card is present and can be initialized:
if (!SD.begin(SDCHIPSELECT)) {
Serial.println("Card failed, or not present");
display.print(F("Fail!")); display.display();
datalogging=false; //disable logging
delay(1000);
}else{
Serial.println("Card initialized.");
display.print(F("LOG=")); display.display();
if (datalogging){
int filenumber=0;
char buffer[6];
sprintf(buffer, "%04d", filenumber);
datalogging_filename="LOG_"+String(buffer)+".TXT";
while(SD.exists(datalogging_filename) && filenumber<10000) {
Serial.print(datalogging_filename); Serial.println(" exists");
filenumber++;
sprintf(buffer, "%04d", filenumber);
datalogging_filename="LOG_"+String(buffer)+".TXT";
}
Serial.print(datalogging_filename); Serial.println(" is free");
display.print(datalogging_filename); display.display();
}
}
}
void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear) {
static unsigned long last_datalogging_write=0;
static boolean logging_headerWritten=false;
if (datalogging) {
#define LOGGINGINTERVAL 100
if (loopmillis-last_datalogging_write>LOGGINGINTERVAL)
{
last_datalogging_write=loopmillis;
File dataFile = SD.open(datalogging_filename, FILE_WRITE);
if (dataFile) { // if the file is available, write to it
if (!logging_headerWritten) {
dataFile.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_RearR,");
dataFile.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
dataFile.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
dataFile.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
dataFile.println("currentAll,throttle,brake,speed,trip,currentConsumed,motorenabled,disarmedByDelay");
dataFile.print("#TIMESTAMP:"); dataFile.println(now());
logging_headerWritten=true;
}
dataFile.print(loopmillis/1000.0,3); dataFile.print(";");
dataFile.print(escFront.getCmdL()); dataFile.print(";");
dataFile.print(escFront.getCmdR()); dataFile.print(";");
dataFile.print(escRear.getCmdL()); dataFile.print(";");
dataFile.print(escRear.getCmdR()); dataFile.print(";");
dataFile.print(escFront.getFiltered_curL(),3); dataFile.print(";");
dataFile.print(escFront.getFiltered_curR(),3); dataFile.print(";");
dataFile.print(escRear.getFiltered_curL(),3); dataFile.print(";");
dataFile.print(escRear.getFiltered_curR(),3); dataFile.print(";");
dataFile.print(escFront.getFeedback_speedL_meas()); dataFile.print(";"); //Todo: check if speed for R wheels needs to be negated
dataFile.print(escFront.getFeedback_speedR_meas()); dataFile.print(";");
dataFile.print(escRear.getFeedback_speedL_meas()); dataFile.print(";");
dataFile.print(escRear.getFeedback_speedR_meas()); dataFile.print(";");
dataFile.print(escFront.getFeedback_boardTemp()); dataFile.print(";");
dataFile.print(escRear.getFeedback_boardTemp()); dataFile.print(";");
dataFile.print(escFront.getFeedback_batVoltage()); dataFile.print(";");
dataFile.print(escRear.getFeedback_batVoltage()); dataFile.print(";");
dataFile.print(filtered_currentAll,3); dataFile.print(";");
dataFile.print(throttle_pos); dataFile.print(";");
dataFile.print(brake_pos); dataFile.print(";");
dataFile.print(meanSpeedms); dataFile.print(";");
dataFile.print(escFront.getTrip()); dataFile.print(";");
dataFile.print(escFront.getCurrentConsumed(),3); dataFile.print(";");
dataFile.println("");
dataFile.close();
}
}
}
}
void writeLogComment(unsigned long time, String msg) {
//SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
if (datalogging) {
File dataFile = SD.open(datalogging_filename, FILE_WRITE);
if (dataFile) { // if the file is available, write to it
dataFile.print("#");
dataFile.print(time/1000.0,3);
dataFile.print(",");
dataFile.print(msg);
dataFile.println();
dataFile.close();
}
}
}
#endif

1
controller_teensy/lib/hoverboard-esc-serial-comm

@ -1 +0,0 @@
Subproject commit 00b432942f437fdbb285f04a1847d3bebc390044

1
controller_teensy/platformio.ini
View File

@ -23,4 +23,3 @@ build_flags =
lib_deps = lib_deps =
robtillaart/ADS1X15@^0.3.9 robtillaart/ADS1X15@^0.3.9
adafruit/Adafruit SSD1306@^2.5.7 adafruit/Adafruit SSD1306@^2.5.7
arduino-libraries/SD@^1.2.4

165
controller_teensy/src/main.cpp
View File

@ -1,22 +1,14 @@
#include <Arduino.h> #include <Arduino.h>
#include "definitions.h" #include "definitions.h"
//#include "structs.h" #include "structs.h"
#include "helpfunctions.h" #include "helpfunctions.h"
#include <TimeLib.h> //for teensy rtc #include <TimeLib.h> //for teensy rtc
#include "hoverboard-esc-serial-comm.h" #include "comms.h"
//#include "comms.h"
#include "display.h" #include "display.h"
#include "logging.h"
#include "ADS1X15.h" #include "ADS1X15.h"
ESCSerialComm escFront(Serial2);
ESCSerialComm escRear(Serial3);
//Serial1 = TX1=1, RX1=0
//Serial2 = TX2=10, RX2=9
//Serial3 = TX3=8, RX3=7
ADS1115 ADS(0x48); ADS1115 ADS(0x48);
/* /*
@ -33,8 +25,7 @@ Tennsy Pin, Pin Name, Connected to
void readADS(); void readADS();
void readADC(); void readADC();
void failChecks(); void failChecks();
//void sendCMD(); void sendCMD();
void calculateSetSpeed();
void checkLog(); void checkLog();
void leds(); void leds();
@ -55,8 +46,8 @@ void setup()
Serial1.begin(SERIAL_LOG_BAUD); //TX1=1, RX1=0 Serial1.begin(SERIAL_LOG_BAUD); //TX1=1, RX1=0
//Serial2.begin(SERIAL_CONTROL_BAUD); //control, TX2=10, RX2=9 Serial2.begin(SERIAL_CONTROL_BAUD); //control, TX2=10, RX2=9
//Serial3.begin(SERIAL_CONTROL_BAUD); //control, TX3=8, RX3=7 Serial3.begin(SERIAL_CONTROL_BAUD); //control, TX3=8, RX3=7
pinMode(PIN_THROTTLE, INPUT); pinMode(PIN_THROTTLE, INPUT);
pinMode(PIN_BRAKE, INPUT); pinMode(PIN_BRAKE, INPUT);
@ -64,8 +55,6 @@ void setup()
pinMode(PIN_LED_START, OUTPUT); //Active High pinMode(PIN_LED_START, OUTPUT); //Active High
//TODO: remove mode button things
pinMode(PIN_MODE_LEDG, OUTPUT); //Active Low pinMode(PIN_MODE_LEDG, OUTPUT); //Active Low
digitalWrite(PIN_MODE_LEDG,LOW); digitalWrite(PIN_MODE_LEDG,LOW);
pinMode(PIN_MODE_LEDR, OUTPUT); //Active Low pinMode(PIN_MODE_LEDR, OUTPUT); //Active Low
@ -73,24 +62,18 @@ void setup()
pinMode(PIN_LATCH_ENABLE, OUTPUT); pinMode(PIN_LATCH_ENABLE, OUTPUT);
digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on
pinMode(PIN_MODE_SWITCH, INPUT_PULLUP);
delay(2000);
Serial.println("Init Functions");
display_init(); display_init();
initLogging();
escFront.init();
escRear.init();
delay(2000); delay(2000);
Serial.println("Wait finished. Booting.."); Serial.println("Wait finished. Booting..");
//init ADS1115 //init ADS1115
if (!ADS.begin()) { if (!ADS.begin()) {
Serial.println("Error:"); delay(2000); Serial.println("ADS1115 Init Error!"); Serial.println("Error:"); delay(2000); Serial.println("ADS1115 Init Error!");
writeLogComment((unsigned long)millis(), "Error ADS1115 Init"); writeLogComment(Serial1,(unsigned long)millis(), "Error ADS1115 Init");
} }
ADS.setGain(0); ADS.setGain(0);
ADS.setDataRate(7);// Read Interval: 7-> 2ms, 6-> 3-4ms , 5-> 5-6ms, 4-> 9ms, 0-> 124ms ADS.setDataRate(7);// Read Interval: 7-> 2ms, 6-> 3-4ms , 5-> 5-6ms, 4-> 9ms, 0-> 124ms
@ -106,12 +89,9 @@ void setup()
} }
writeLogComment(millis(), "Setup Finished");
} }
unsigned long loopmillis; unsigned long loopmillis;
// ########################## LOOP ########################## // ########################## LOOP ##########################
void loop() { void loop() {
//Serial.print("Loopduration="); Serial.println(); //loopduration is at max 11ms //Serial.print("Loopduration="); Serial.println(); //loopduration is at max 11ms
@ -119,7 +99,6 @@ void loop() {
loopmillis=millis(); //read millis for this cycle loopmillis=millis(); //read millis for this cycle
/*
// ____ Debugging pending serial byted for feedback // ____ Debugging pending serial byted for feedback
static int s2availmax=0; static int s2availmax=0;
int _a2=Serial2.available(); int _a2=Serial2.available();
@ -154,8 +133,6 @@ void loop() {
if (newData3) { if (newData3) {
updateMotorparams(motorparamsRear,FeedbackRear,loopmillis); updateMotorparams(motorparamsRear,FeedbackRear,loopmillis);
} }
*/
@ -186,19 +163,9 @@ void loop() {
failChecks(); failChecks();
static unsigned long last_calculateSetSpeed=0;
if (loopmillis - last_calculateSetSpeed > SENDPERIOD) {
calculateSetSpeed();
}
escFront.update(loopmillis);
escRear.update(loopmillis);
/* TODO: remove this if, because everything contained in esc.update()
if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers
last_send=loopmillis; last_send=loopmillis;
//sendCMD(); sendCMD();
//Update speed and trip //Update speed and trip
float _meanRPM=(FeedbackFront.speedL_meas-FeedbackFront.speedR_meas+FeedbackRear.speedL_meas-FeedbackRear.speedR_meas)/4.0; float _meanRPM=(FeedbackFront.speedL_meas-FeedbackFront.speedR_meas+FeedbackRear.speedL_meas-FeedbackRear.speedR_meas)/4.0;
@ -208,18 +175,16 @@ void loop() {
//mah consumed //mah consumed
currentConsumed += (motorparamsFront.filtered_curL+motorparamsFront.filtered_curR+motorparamsRear.filtered_curL+motorparamsRear.filtered_curR)* (SENDPERIOD/1000.0)/3600.0; //amp hours currentConsumed += (motorparamsFront.filtered_curL+motorparamsFront.filtered_curR+motorparamsRear.filtered_curL+motorparamsRear.filtered_curR)* (SENDPERIOD/1000.0)/3600.0; //amp hours
} }
*/
//If needed write log to serial port //If needed write log to serial port
//checkLog(); //TODO remove checkLog();
loggingLoop(loopmillis,escFront,escRear);
leds(); leds();
static unsigned long last_display_update=0; static unsigned long last_display_update=0;
if (loopmillis - last_display_update > DISPLAYUPDATEPERIOD) { if (loopmillis - last_display_update > DISPLAYUPDATEPERIOD) {
last_display_update=loopmillis; last_display_update=loopmillis;
display_update(); display_test();
} }
} }
@ -298,13 +263,13 @@ void readADC() {
//Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", "); //Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
//Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println(); //Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
/*
if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected
if (speedmode!=SPEEDMODE_SLOW) { if (speedmode!=SPEEDMODE_SLOW) {
speedmode=SPEEDMODE_SLOW; speedmode=SPEEDMODE_SLOW;
max_acceleration_rate=SLOW_MAX_ACCELERATION_RATE; max_acceleration_rate=SLOW_MAX_ACCELERATION_RATE;
if (loopmillis>WRITE_HEADER_TIME) { if (loopmillis>WRITE_HEADER_TIME) {
//writeLogComment(Serial1,loopmillis, "Mode switched to SPEEDMODE_SLOW"); writeLogComment(Serial1,loopmillis, "Mode switched to SPEEDMODE_SLOW");
} }
} }
}else{ //button not pushed in }else{ //button not pushed in
@ -312,24 +277,20 @@ void readADC() {
speedmode=SPEEDMODE_NORMAL; speedmode=SPEEDMODE_NORMAL;
max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE;
if (loopmillis>WRITE_HEADER_TIME) { if (loopmillis>WRITE_HEADER_TIME) {
//writeLogComment(Serial1,loopmillis, "Mode switched to SPEEDMODE_NORMAL"); writeLogComment(Serial1,loopmillis, "Mode switched to SPEEDMODE_NORMAL");
} }
} }
} }
*/
/*
if (speedmode==SPEEDMODE_SLOW) { if (speedmode==SPEEDMODE_SLOW) {
throttle_pos/=2; throttle_pos/=2;
} }
*/
} }
void failChecks() { void failChecks() {
/*
if ( loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected if ( loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected
if (controllerFront_connected) { //just got disconnected if (controllerFront_connected) { //just got disconnected
controllerFront_connected=false; controllerFront_connected=false;
@ -350,9 +311,9 @@ void failChecks() {
}else if(!controllerRear_connected && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before }else if(!controllerRear_connected && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before
controllerRear_connected=true; controllerRear_connected=true;
writeLogComment(Serial1,loopmillis, "Controller Rear connected"); writeLogComment(Serial1,loopmillis, "Controller Rear connected");
}*/ }
controllers_connected=escFront.getControllerConnected() & escRear.getControllerConnected(); controllers_connected=controllerFront_connected & controllerRear_connected;
//ADC Range Check //ADC Range Check
if ((throttle_raw >= failsafe_throttle_min) & (throttle_raw <= failsafe_throttle_max)) { //inside safe range (to check if wire got disconnected) if ((throttle_raw >= failsafe_throttle_min) & (throttle_raw <= failsafe_throttle_max)) { //inside safe range (to check if wire got disconnected)
@ -361,7 +322,7 @@ void failChecks() {
if (loopmillis>throttle_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long if (loopmillis>throttle_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
if (!error_throttle_outofrange) { if (!error_throttle_outofrange) {
error_throttle_outofrange=true; error_throttle_outofrange=true;
writeLogComment(loopmillis, "Error Throttle ADC Out of Range"); writeLogComment(Serial1,loopmillis, "Error Throttle ADC Out of Range");
} }
//Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw); //Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw);
@ -372,7 +333,8 @@ void failChecks() {
if (loopmillis>brake_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long if (loopmillis>brake_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
if(!error_brake_outofrange) { if(!error_brake_outofrange) {
error_brake_outofrange=true; error_brake_outofrange=true;
writeLogComment(loopmillis, "Error Brake ADC Out of Range"); writeLogComment(Serial1,loopmillis, "Error Brake ADC Out of Range");
} }
//Serial.print("Error Brake ADC Out of Range="); Serial.println(brake_raw); //Serial.print("Error Brake ADC Out of Range="); Serial.println(brake_raw);
} }
@ -380,7 +342,7 @@ void failChecks() {
if (loopmillis-last_adsread > ADS_MAX_READ_INTERVAL) { if (loopmillis-last_adsread > ADS_MAX_READ_INTERVAL) {
if (!error_ads_max_read_interval) { if (!error_ads_max_read_interval) {
error_ads_max_read_interval=true; error_ads_max_read_interval=true;
writeLogComment(loopmillis, "Error ADS Max read interval"); writeLogComment(Serial1,loopmillis, "Error ADS Max read interval");
} }
//Serial.print("Error ADS Max read interval=");Serial.println(loopmillis-last_adsread); //Serial.print("Error ADS Max read interval=");Serial.println(loopmillis-last_adsread);
} }
@ -391,64 +353,13 @@ void failChecks() {
} }
} }
void calculateSetSpeed(){ void sendCMD() {
/* ## FOR REFERENCE:
int16_t brake_pos_expo = (int16_t)(pow((brake_pos/1000.0),2)*1000); int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking
int16_t brake_cmdreduce_proportional=100; //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=brake_cmdreduce_proportional / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling)
float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000) float startbrakecurrent=3; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
int16_t cmdreduce_constant=map(brake_pos_expo,0,1000,0,(int16_t)(brake_cmdreduce_proportional*SENDPERIOD/1000)); //reduce cmd value every cycle */
float freewheel_current=startbrakecurrent_offset-brake_pos_expo*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling
float freewheel_break_factor=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (on average)
float filtered_currentFront=max(escFront.getFiltered_curL(),escFront.getFiltered_curR());
float filtered_currentRear=max(escRear.getFiltered_curL(),escRear.getFiltered_curR());
filtered_currentAll=max(filtered_currentFront,filtered_currentRear); //positive value is current Drawn from battery. negative value is braking current
if (throttle_pos>=last_cmd_send) { //accelerating
cmd_send += constrain(throttle_pos-cmd_send,0,max_acceleration_rate*SENDPERIOD/1000); //if throttle higher than last applied value, apply throttle directly
}else{ //freewheeling or braking
if (filtered_currentAll>freewheel_current) { //drive current too high
cmd_send-= max(0, (filtered_currentAll-freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value
}
cmd_send-=max(minimum_constant_cmd_reduce,cmdreduce_constant); //reduce slowly anyways
}
cmd_send=constrain(cmd_send,0,1000);
last_cmd_send=cmd_send;
int16_t cmd_send_toMotor=constrain(cmd_send* (1.0-(brake_pos*0.5/1000.0) ) ,0,1000); //brake "ducking"
if (reverse_enabled) {
cmd_send_toMotor-=brake_pos*REVERSE_SPEED;
}
if (!controllers_connected || !armed) { //controllers not connected or not armed
cmd_send=0;
cmd_send_toMotor=0; //safety off
}
escFront.setSpeed(cmd_send_toMotor,cmd_send_toMotor);
escRear.setSpeed(cmd_send_toMotor,cmd_send_toMotor);
log_update=true;
}
/*
void sendCMD() { //TODO: remove complete function because replaced by calculateSetSpeed()
// ## FOR REFERENCE:
//int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking
//int16_t brake_cmdreduce_proportional=100; //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=brake_cmdreduce_proportional / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling)
//float startbrakecurrent=3; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
//float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
int16_t brake_pos_expo = (int16_t)(pow((brake_pos/1000.0),2)*1000); int16_t brake_pos_expo = (int16_t)(pow((brake_pos/1000.0),2)*1000);
float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000) float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
@ -496,32 +407,27 @@ void sendCMD() { //TODO: remove complete function because replaced by calculateS
} }
//apply throttle command to all motors //apply throttle command to all motors
motorparamsFront.cmdL=cmd_send_toMotor; motorparamsFront.cmdL=cmd_send_toMotor;
motorparamsFront.cmdR=cmd_send_toMotor; motorparamsFront.cmdR=cmd_send_toMotor;
motorparamsRear.cmdL=cmd_send_toMotor; motorparamsRear.cmdL=cmd_send_toMotor;
motorparamsRear.cmdR=cmd_send_toMotor; motorparamsRear.cmdR=cmd_send_toMotor;
if (controllers_connected) { if (controllers_connected) {
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2); SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3); SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
log_update=true; log_update=true;
//Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000); Serial.println() //Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000); Serial.println()
}//else if(loopmillis>last_log_send+LOGMININTERVAL){ }/*else if(loopmillis>last_log_send+LOGMININTERVAL){
// //Serial.print(throttle_raw); Serial.println(); //Serial.print(throttle_raw); Serial.println();
// Serial.print(linearizeThrottle(throttle_raw)); Serial.println(); Serial.print(linearizeThrottle(throttle_raw)); Serial.println();
// last_log_send=loopmillis; last_log_send=loopmillis;
//}
}*/ }*/
}
/* void checkLog() {
void checkLog() { //TODO: remove
if (!log_header_written && loopmillis>=WRITE_HEADER_TIME){ //write header for log file after logger booted up if (!log_header_written && loopmillis>=WRITE_HEADER_TIME){ //write header for log file after logger booted up
writeLogInfo(Serial1); writeLogInfo(Serial1);
writeLogHeader(Serial1); writeLogHeader(Serial1);
@ -534,7 +440,6 @@ void checkLog() { //TODO: remove
writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos); writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
} }
} }
*/
@ -572,11 +477,11 @@ void readButtons() {
if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long
if (throttle_pos<=0 && brake_pos<=0 && controllers_connected && !armed) { //brake or thottle not pressed, controllers connected if (throttle_pos<=0 && brake_pos<=0 && controllers_connected && !armed) { //brake or thottle not pressed, controllers connected
armed=true; //arm if button pressed long enough armed=true; //arm if button pressed long enough
writeLogComment(loopmillis, "Armed by button"); writeLogComment(Serial1,loopmillis, "Armed by button");
} }
}else if (armed){ //not pressed long enough and is armed }else if (armed){ //not pressed long enough and is armed
armed=false; //disarm armed=false; //disarm
writeLogComment(loopmillis, "Disarmed by button"); writeLogComment(Serial1,loopmillis, "Disarmed by button");
} }
} }