Adding RPM and speed calc, changing receiver mac

This commit is contained in:
Lukas Bachschwell 2018-05-08 23:23:39 +02:00
parent a13936c35e
commit a17572fa1a
5 changed files with 116 additions and 17 deletions

View File

@ -28,9 +28,8 @@ framework = arduino
src_filter = +<receiver.cpp> src_filter = +<receiver.cpp>
monitor_baud = 115200 monitor_baud = 115200
upload_speed = 921600 ; upload_speed = 921600
lib_deps = lib_deps =
# Using a library name
ServoESP32 ServoESP32
DallasTemperature DallasTemperature

View File

@ -1,7 +1,7 @@
//mac addresses //mac addresses
// reciever mac: // reciever mac:
uint8_t mac_receiver[] = {0x30, 0xae, 0xa4, 0x05, 0x82, 0x15}; uint8_t mac_receiver[] = {0xB4, 0xE6, 0x2D, 0x8C, 0x34, 0xAE};
// remote macStr // remote macStr
uint8_t mac_remote[] = {0x24, 0x0a, 0xc4, 0x82, 0x51, 0x70}; uint8_t mac_remote[] = {0x24, 0x0a, 0xc4, 0x82, 0x51, 0x70};

View File

@ -36,14 +36,16 @@ esp_now_peer_info_t remote;
float temperature = 0; float temperature = 0;
float voltage = 0; float voltage = 0;
float speed = 0; float kmh = 0;
uint8_t sendTemperature = 0; uint8_t sendTemperature = 0;
uint8_t sendTemperatureDecimals = 0; uint8_t sendTemperatureDecimals = 0;
uint8_t sendVoltage = 0; uint8_t sendVoltage = 0;
uint8_t sendVoltageDecimals = 0; uint8_t sendVoltageDecimals = 0;
uint8_t sendRPMupper = 1; uint8_t sendSpeed = 0;
uint8_t sendRPMlower = 2; uint8_t sendSpeedDecimals = 0;
#include "rpm.h"
//#define pairingMode //#define pairingMode
#define CONNECTION_TIMEOUT 300 #define CONNECTION_TIMEOUT 300
@ -80,9 +82,9 @@ void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
// Answer with response // Answer with response
const uint8_t respData[] = { sendVoltage, sendVoltageDecimals, sendTemperature, sendTemperatureDecimals, sendRPMupper, sendRPMlower }; const uint8_t respData[] = { sendVoltage, sendVoltageDecimals, sendTemperature, sendTemperatureDecimals, sendSpeed, sendSpeedDecimals };
Serial.print("Sending RESPONSE.... "); Serial.print("Sending RESPONSE.... ");
esp_err_t result = esp_now_send(mac_addr, respData, sizeof(data)); esp_err_t result = esp_now_send(mac_addr, respData, sizeof(respData));
if (result == ESP_OK) { if (result == ESP_OK) {
Serial.println("Success"); Serial.println("Success");
} else if (result == ESP_ERR_ESPNOW_NOT_INIT) { } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
@ -192,8 +194,8 @@ bool manageRemote() {
void checkTemperature() { void checkTemperature() {
sensors.requestTemperatures(); // Send the command to get temperatures sensors.requestTemperatures(); // Send the command to get temperatures
temperature = sensors.getTempCByIndex(0); temperature = sensors.getTempCByIndex(0);
Serial.print("Temp: "); //Serial.print("Temp: ");
Serial.println(temperature); //Serial.println(temperature);
if(temperature < 35) digitalWrite(fanRelais, LOW); if(temperature < 35) digitalWrite(fanRelais, LOW);
if(temperature > 40) digitalWrite(fanRelais, HIGH); if(temperature > 40) digitalWrite(fanRelais, HIGH);
sendTemperature = abs(floor(temperature)); sendTemperature = abs(floor(temperature));
@ -202,9 +204,9 @@ void checkTemperature() {
void checkVoltage() { void checkVoltage() {
digitalWrite(voltageEnable, HIGH); digitalWrite(voltageEnable, HIGH);
Serial.print("Voltage: "); //Serial.print("Voltage: ");
int value = analogRead(voltagePin); int value = analogRead(voltagePin);
Serial.println(value); //Serial.println(value);
float batteryVoltage = 0.0; float batteryVoltage = 0.0;
@ -220,8 +222,8 @@ void checkVoltage() {
sendVoltage = abs(floor(batteryVoltage)); sendVoltage = abs(floor(batteryVoltage));
sendVoltageDecimals = (batteryVoltage - sendVoltage) * 100; sendVoltageDecimals = (batteryVoltage - sendVoltage) * 100;
Serial.print("Voltage: "); //Serial.print("Voltage: ");
Serial.println(batteryVoltage); //Serial.println(batteryVoltage);
digitalWrite(voltageEnable, LOW); // change to low digitalWrite(voltageEnable, LOW); // change to low
} }
@ -237,6 +239,8 @@ void setup() {
pinMode(voltageEnable, OUTPUT); pinMode(voltageEnable, OUTPUT);
pinMode(fanRelais, OUTPUT); pinMode(fanRelais, OUTPUT);
initRPMPins();
// Init escs, min and max value similar as Traxxas TQI 1100, 1900 // Init escs, min and max value similar as Traxxas TQI 1100, 1900
// chanel, minAngel, maxAngel, minPulseWidth, maxPulseWidth // chanel, minAngel, maxAngel, minPulseWidth, maxPulseWidth
esc1.attach(esc1pin, 0, 0, 255, 1100, 1900); esc1.attach(esc1pin, 0, 0, 255, 1100, 1900);
@ -244,6 +248,15 @@ void setup() {
sensors.begin(); sensors.begin();
xTaskCreatePinnedToCore(
measureRpm,
"rpm task",
1000,
NULL,
1,
&rpmTaskHandle,
0);
//Set device in AP mode to begin with //Set device in AP mode to begin with
WiFi.mode(WIFI_AP); WiFi.mode(WIFI_AP);

View File

@ -565,7 +565,6 @@ void drawPage() {
switch (displayData) { switch (displayData) {
case 0: case 0:
//value = ratioRpmSpeed * data.rpm;
first = boardData[B_TEMP]; first = boardData[B_TEMP];
last = boardData[B_TEMP_D]; last = boardData[B_TEMP_D];
suffix = "C"; suffix = "C";
@ -573,8 +572,6 @@ void drawPage() {
decimals = 2; decimals = 2;
break; break;
case 1: case 1:
//value = ratioPulseDistance * data.tachometerAbs;
// TODO : check how that will work once hal is wired
first = boardData[B_SPEED]; first = boardData[B_SPEED];
last = boardData[B_SPEED_D]; last = boardData[B_SPEED_D];
suffix = "KmH"; suffix = "KmH";

90
src/rpm.h Normal file
View File

@ -0,0 +1,90 @@
#define halBlueLeft 2
#define halYellowLeft 22
#define halGreenLeft 19
#define halBlueRight 23
#define halYellowRight 18
#define halGreenRight 5
#define wheelCircumference 26.062
TaskHandle_t rpmTaskHandle;
int rpmRight = 0;
int rpmLeft = 0;
int lastState[2] = {1, 1};
long lastTime[2] = {0, 0};
int counter[2] = {0, 0};
long rpmTimer = 0;
// Zero states are non existant
int motorPosition[2][2][2] {
{
{0, 1},
{3, 2}
},
{
{5, 6},
{4, 0}
}
};
// old state, new state
int relativeLookup[6][6] {
{ 0, 1, 2, 3,-2,-1}, // 1
{-1, 0, 1, 2, 3,-1}, // 2
{-2,-1, 0, 1, 2, 3}, // 3
{-3,-2,-1, 0, 1, 2}, // 4
{ 1,-3,-2,-1, 0, 1}, // 5
{ 1, 2,-3,-2,-1, 0} // 6
};
int getState(bool isLeft) {
if(isLeft) {
return motorPosition[digitalRead(halBlueLeft)][digitalRead(halYellowLeft)][digitalRead(halGreenLeft)];
} else {
return motorPosition[digitalRead(halBlueRight)][digitalRead(halYellowRight)][digitalRead(halGreenRight)];
}
}
void initRPMPins() {
pinMode(halBlueLeft, INPUT_PULLUP);
pinMode(halYellowLeft, INPUT_PULLUP);
pinMode(halGreenLeft, INPUT_PULLUP);
pinMode(halBlueRight, INPUT_PULLUP);
pinMode(halYellowRight, INPUT_PULLUP);
pinMode(halGreenRight, INPUT_PULLUP);
}
void measureRpm(void * parameter) {
while(true) {
if(millis()-rpmTimer>250) {
rpmRight = counter[false]*4;
rpmLeft = counter[true]*4;
kmh = (wheelCircumference/100) * (rpmRight+rpmLeft)/2 * 60/1000;
sendSpeed = abs(floor(kmh));
sendSpeedDecimals = (kmh - sendSpeed) * 100;
Serial.print("remote "); Serial.println(kmh);
counter[true] = 0;
counter[false] = 0;
rpmTimer = millis();
}
int state = getState(false);
if(lastState[false] != state) {
counter[false] = counter[false] + relativeLookup[lastState[false]-1][state-1];
lastState[false] = state;
}
state = getState(true);
if(lastState[true] != state) {
counter[true] = counter[true] + relativeLookup[lastState[true]-1][state-1];
lastState[true] = state;
}
vTaskDelay(pdMS_TO_TICKS(1));
}
}