modify code to use hoverboard-esc-serial-comm and add sd card via SPI. untested
This commit is contained in:
parent
49f6d1c992
commit
7516b76d2a
|
|
@ -106,7 +106,7 @@ void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _lo
|
|||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
void writeLogInfo(HardwareSerial &SerialRef) { //first line of file
|
||||
SerialRef.print("#TIMESTAMP:");
|
||||
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();
|
||||
}
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#endif
|
||||
|
|
@ -11,7 +11,7 @@
|
|||
bool log_update=true;
|
||||
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 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;
|
||||
|
||||
|
||||
#define FEEDBACKRECEIVETIMEOUT 500
|
||||
//#define FEEDBACKRECEIVETIMEOUT 500
|
||||
|
||||
bool controllerFront_connected=false;
|
||||
bool controllerRear_connected=false;
|
||||
//bool controllerFront_connected=false;
|
||||
//bool controllerRear_connected=false;
|
||||
bool controllers_connected=false;
|
||||
|
||||
#define PIN_THROTTLE A7
|
||||
|
|
@ -90,14 +90,13 @@ unsigned long last_notidle=0; //not rolling to fast, no pedal pressed
|
|||
|
||||
#define PIN_LATCH_ENABLE A6
|
||||
|
||||
#define PIN_MODE_SWITCH 3
|
||||
#define PIN_MODE_LEDG 4
|
||||
#define PIN_MODE_LEDR 5
|
||||
|
||||
|
||||
|
||||
unsigned long last_send = 0;
|
||||
unsigned long last_receive = 0;
|
||||
//unsigned long last_send = 0;
|
||||
//unsigned long last_receive = 0;
|
||||
|
||||
float filtered_currentAll=0;
|
||||
|
||||
|
|
@ -123,4 +122,7 @@ bool armed = false; //cmd output values forced to 0 if false
|
|||
|
||||
#define DISPLAYUPDATEPERIOD 100
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
|
@ -13,24 +13,30 @@
|
|||
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
|
||||
|
||||
void display_init();
|
||||
void display_test();
|
||||
void display_update();
|
||||
|
||||
void display_init(){
|
||||
if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
|
||||
Serial.println(F("SSD1306 allocation failed"));
|
||||
}
|
||||
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();
|
||||
}
|
||||
|
||||
void display_test(){
|
||||
void display_update(){
|
||||
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.println(F("Hello Welt"));
|
||||
display.println(millis());
|
||||
display.println(now());
|
||||
display.print(F("Speed : ")); display.println(meanSpeedms);
|
||||
display.print(F("Throttle: ")); display.println(throttle_pos);
|
||||
display.print(F("Brake : ")); display.println(brake_pos);
|
||||
display.print(F("Current : ")); display.println(filtered_currentAll);
|
||||
|
||||
|
||||
display.display();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#ifndef _HELPFUNCTIONS_H_
|
||||
#define _HELPFUNCTIONS_H_
|
||||
|
||||
#include "definitions.h"
|
||||
//#include "definitions.h"
|
||||
|
||||
int sort_desc(const void *cmp1, const void *cmp2);
|
||||
float filterMedian(int16_t* values);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,115 @@
|
|||
#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 +1 @@
|
|||
Subproject commit 91bfeffcb33448f45b17a753ef8b798886e63af1
|
||||
Subproject commit 00b432942f437fdbb285f04a1847d3bebc390044
|
||||
|
|
@ -23,3 +23,4 @@ build_flags =
|
|||
lib_deps =
|
||||
robtillaart/ADS1X15@^0.3.9
|
||||
adafruit/Adafruit SSD1306@^2.5.7
|
||||
arduino-libraries/SD@^1.2.4
|
||||
|
|
@ -1,14 +1,22 @@
|
|||
#include <Arduino.h>
|
||||
|
||||
#include "definitions.h"
|
||||
#include "structs.h"
|
||||
//#include "structs.h"
|
||||
#include "helpfunctions.h"
|
||||
#include <TimeLib.h> //for teensy rtc
|
||||
#include "comms.h"
|
||||
#include "hoverboard-esc-serial-comm.h"
|
||||
//#include "comms.h"
|
||||
#include "display.h"
|
||||
|
||||
#include "logging.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);
|
||||
|
||||
/*
|
||||
|
|
@ -25,7 +33,8 @@ Tennsy Pin, Pin Name, Connected to
|
|||
void readADS();
|
||||
void readADC();
|
||||
void failChecks();
|
||||
void sendCMD();
|
||||
//void sendCMD();
|
||||
void calculateSetSpeed();
|
||||
void checkLog();
|
||||
|
||||
void leds();
|
||||
|
|
@ -46,8 +55,8 @@ void setup()
|
|||
|
||||
Serial1.begin(SERIAL_LOG_BAUD); //TX1=1, RX1=0
|
||||
|
||||
Serial2.begin(SERIAL_CONTROL_BAUD); //control, TX2=10, RX2=9
|
||||
Serial3.begin(SERIAL_CONTROL_BAUD); //control, TX3=8, RX3=7
|
||||
//Serial2.begin(SERIAL_CONTROL_BAUD); //control, TX2=10, RX2=9
|
||||
//Serial3.begin(SERIAL_CONTROL_BAUD); //control, TX3=8, RX3=7
|
||||
|
||||
pinMode(PIN_THROTTLE, INPUT);
|
||||
pinMode(PIN_BRAKE, INPUT);
|
||||
|
|
@ -55,6 +64,8 @@ void setup()
|
|||
|
||||
pinMode(PIN_LED_START, OUTPUT); //Active High
|
||||
|
||||
|
||||
//TODO: remove mode button things
|
||||
pinMode(PIN_MODE_LEDG, OUTPUT); //Active Low
|
||||
digitalWrite(PIN_MODE_LEDG,LOW);
|
||||
pinMode(PIN_MODE_LEDR, OUTPUT); //Active Low
|
||||
|
|
@ -62,18 +73,24 @@ void setup()
|
|||
|
||||
pinMode(PIN_LATCH_ENABLE, OUTPUT);
|
||||
digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on
|
||||
pinMode(PIN_MODE_SWITCH, INPUT_PULLUP);
|
||||
|
||||
delay(2000);
|
||||
Serial.println("Init Functions");
|
||||
|
||||
display_init();
|
||||
|
||||
initLogging();
|
||||
|
||||
escFront.init();
|
||||
escRear.init();
|
||||
|
||||
delay(2000);
|
||||
Serial.println("Wait finished. Booting..");
|
||||
|
||||
//init ADS1115
|
||||
if (!ADS.begin()) {
|
||||
Serial.println("Error:"); delay(2000); Serial.println("ADS1115 Init Error!");
|
||||
writeLogComment(Serial1,(unsigned long)millis(), "Error ADS1115 Init");
|
||||
writeLogComment((unsigned long)millis(), "Error ADS1115 Init");
|
||||
}
|
||||
ADS.setGain(0);
|
||||
ADS.setDataRate(7);// Read Interval: 7-> 2ms, 6-> 3-4ms , 5-> 5-6ms, 4-> 9ms, 0-> 124ms
|
||||
|
|
@ -89,9 +106,12 @@ void setup()
|
|||
}
|
||||
|
||||
|
||||
writeLogComment(millis(), "Setup Finished");
|
||||
|
||||
}
|
||||
|
||||
unsigned long loopmillis;
|
||||
|
||||
// ########################## LOOP ##########################
|
||||
void loop() {
|
||||
//Serial.print("Loopduration="); Serial.println(); //loopduration is at max 11ms
|
||||
|
|
@ -99,6 +119,7 @@ void loop() {
|
|||
loopmillis=millis(); //read millis for this cycle
|
||||
|
||||
|
||||
/*
|
||||
// ____ Debugging pending serial byted for feedback
|
||||
static int s2availmax=0;
|
||||
int _a2=Serial2.available();
|
||||
|
|
@ -133,6 +154,8 @@ void loop() {
|
|||
if (newData3) {
|
||||
updateMotorparams(motorparamsRear,FeedbackRear,loopmillis);
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -163,9 +186,19 @@ void loop() {
|
|||
|
||||
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
|
||||
last_send=loopmillis;
|
||||
sendCMD();
|
||||
//sendCMD();
|
||||
|
||||
|
||||
//Update speed and trip
|
||||
float _meanRPM=(FeedbackFront.speedL_meas-FeedbackFront.speedR_meas+FeedbackRear.speedL_meas-FeedbackRear.speedR_meas)/4.0;
|
||||
|
|
@ -175,16 +208,18 @@ void loop() {
|
|||
//mah consumed
|
||||
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
|
||||
checkLog();
|
||||
//checkLog(); //TODO remove
|
||||
loggingLoop(loopmillis,escFront,escRear);
|
||||
|
||||
leds();
|
||||
|
||||
static unsigned long last_display_update=0;
|
||||
if (loopmillis - last_display_update > DISPLAYUPDATEPERIOD) {
|
||||
last_display_update=loopmillis;
|
||||
display_test();
|
||||
display_update();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -263,13 +298,13 @@ void readADC() {
|
|||
//Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
|
||||
//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 (speedmode!=SPEEDMODE_SLOW) {
|
||||
speedmode=SPEEDMODE_SLOW;
|
||||
max_acceleration_rate=SLOW_MAX_ACCELERATION_RATE;
|
||||
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
|
||||
|
|
@ -277,20 +312,24 @@ void readADC() {
|
|||
speedmode=SPEEDMODE_NORMAL;
|
||||
max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE;
|
||||
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) {
|
||||
throttle_pos/=2;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
}
|
||||
|
||||
void failChecks() {
|
||||
/*
|
||||
if ( loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected
|
||||
if (controllerFront_connected) { //just got disconnected
|
||||
controllerFront_connected=false;
|
||||
|
|
@ -311,9 +350,9 @@ void failChecks() {
|
|||
}else if(!controllerRear_connected && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before
|
||||
controllerRear_connected=true;
|
||||
writeLogComment(Serial1,loopmillis, "Controller Rear connected");
|
||||
}
|
||||
}*/
|
||||
|
||||
controllers_connected=controllerFront_connected & controllerRear_connected;
|
||||
controllers_connected=escFront.getControllerConnected() & escRear.getControllerConnected();
|
||||
|
||||
//ADC Range Check
|
||||
if ((throttle_raw >= failsafe_throttle_min) & (throttle_raw <= failsafe_throttle_max)) { //inside safe range (to check if wire got disconnected)
|
||||
|
|
@ -322,7 +361,7 @@ void failChecks() {
|
|||
if (loopmillis>throttle_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
|
||||
if (!error_throttle_outofrange) {
|
||||
error_throttle_outofrange=true;
|
||||
writeLogComment(Serial1,loopmillis, "Error Throttle ADC Out of Range");
|
||||
writeLogComment(loopmillis, "Error Throttle ADC Out of Range");
|
||||
|
||||
}
|
||||
//Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw);
|
||||
|
|
@ -333,8 +372,7 @@ void failChecks() {
|
|||
if (loopmillis>brake_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
|
||||
if(!error_brake_outofrange) {
|
||||
error_brake_outofrange=true;
|
||||
writeLogComment(Serial1,loopmillis, "Error Brake ADC Out of Range");
|
||||
|
||||
writeLogComment(loopmillis, "Error Brake ADC Out of Range");
|
||||
}
|
||||
//Serial.print("Error Brake ADC Out of Range="); Serial.println(brake_raw);
|
||||
}
|
||||
|
|
@ -342,7 +380,7 @@ void failChecks() {
|
|||
if (loopmillis-last_adsread > ADS_MAX_READ_INTERVAL) {
|
||||
if (!error_ads_max_read_interval) {
|
||||
error_ads_max_read_interval=true;
|
||||
writeLogComment(Serial1,loopmillis, "Error ADS Max read interval");
|
||||
writeLogComment(loopmillis, "Error ADS Max read interval");
|
||||
}
|
||||
//Serial.print("Error ADS Max read interval=");Serial.println(loopmillis-last_adsread);
|
||||
}
|
||||
|
|
@ -353,13 +391,64 @@ void failChecks() {
|
|||
}
|
||||
}
|
||||
|
||||
void sendCMD() {
|
||||
/* ## 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
|
||||
*/
|
||||
void calculateSetSpeed(){
|
||||
|
||||
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)
|
||||
|
||||
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);
|
||||
|
||||
float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
|
||||
|
|
@ -407,27 +496,32 @@ void sendCMD() {
|
|||
}
|
||||
|
||||
//apply throttle command to all motors
|
||||
|
||||
motorparamsFront.cmdL=cmd_send_toMotor;
|
||||
motorparamsFront.cmdR=cmd_send_toMotor;
|
||||
motorparamsRear.cmdL=cmd_send_toMotor;
|
||||
motorparamsRear.cmdR=cmd_send_toMotor;
|
||||
|
||||
|
||||
if (controllers_connected) {
|
||||
|
||||
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
|
||||
SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
|
||||
|
||||
|
||||
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()
|
||||
|
||||
}/*else if(loopmillis>last_log_send+LOGMININTERVAL){
|
||||
//Serial.print(throttle_raw); Serial.println();
|
||||
Serial.print(linearizeThrottle(throttle_raw)); Serial.println();
|
||||
last_log_send=loopmillis;
|
||||
}//else if(loopmillis>last_log_send+LOGMININTERVAL){
|
||||
// //Serial.print(throttle_raw); Serial.println();
|
||||
// Serial.print(linearizeThrottle(throttle_raw)); Serial.println();
|
||||
// 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
|
||||
writeLogInfo(Serial1);
|
||||
writeLogHeader(Serial1);
|
||||
|
|
@ -440,6 +534,7 @@ void checkLog() {
|
|||
writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
|
@ -477,11 +572,11 @@ void readButtons() {
|
|||
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
|
||||
armed=true; //arm if button pressed long enough
|
||||
writeLogComment(Serial1,loopmillis, "Armed by button");
|
||||
writeLogComment(loopmillis, "Armed by button");
|
||||
}
|
||||
}else if (armed){ //not pressed long enough and is armed
|
||||
armed=false; //disarm
|
||||
writeLogComment(Serial1,loopmillis, "Disarmed by button");
|
||||
writeLogComment(loopmillis, "Disarmed by button");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue