bobbycar/controller_teensy/include/rc.h

124 lines
3.1 KiB
C
Raw Normal View History

2024-09-21 20:10:51 +00:00
#ifndef _RC_H_
#define _RC_H_
#define RC
#include <PulsePosition.h>
PulsePositionInput ppmIn;
#define PPMREADPERIOD 20 //period for d4r-ii is 18ms
static int count=0;
#define PIN_PPM 9
#define PPM_CHANNELS 8
#define PPM_SAVE_MIN 950 //minimum allowed for valid data
#define PPM_SAVE_MAX 2050 //maximum allowed for valid data
#define PPM_MIN 1000
#define PPM_MAX 2000
#define PPM_MID 1500
#define PPM_DEADBAND 5
#define PPM_CHANNEL_THROTTLE 1
#define PPM_CHANNEL_STEER 2
#define PPM_CHANNEL_MODE 5
float ppm[PPM_CHANNELS];
unsigned long ppm_count_success=0;
unsigned long ppm_count_fail=0;
void initRC();
void readPPM(unsigned long loopmillis);
void printPPM();
int16_t getRCThrottle();
int16_t getRCSteer();
uint8_t getRCMode(uint8_t modes);
bool rc_enabled=false;
void initRC() {
ppmIn.begin(PIN_PPM);
}
void readPPM(unsigned long loopmillis) {
static unsigned long ppm_last_received=0;
static unsigned long ppm_last_received_success=0;
int ppm_num = ppmIn.available();
if (ppm_num > 0) {
bool ppm_ok=true;
ppm_last_received=loopmillis;
float new_ppm[PPM_CHANNELS];
if (ppm_num != PPM_CHANNELS) {
ppm_ok=false;
}else{
for (int i=1; i <= ppm_num; i++) {
float val=ppmIn.read(i);
new_ppm[i-1]=val;
if (val<PPM_SAVE_MIN || val>PPM_SAVE_MAX) {
ppm_ok=false;
}
}
}
if (ppm_ok) {
ppm_count_success+=1;
ppm_last_received_success=loopmillis;
std::copy(std::begin(new_ppm), std::end(new_ppm), std::begin(ppm));
}else{
ppm_count_fail+=1;
}
}
}
float getPPMSuccessrate() {
float rate=ppm_count_success*1.0/(ppm_count_success+ppm_count_fail);
if (ppm_count_success+ppm_count_fail>500) {
ppm_count_success=0;
ppm_count_fail=0;
}
return rate;
}
//Return value 0 - 1000
int16_t getRCThrottle() {
return constrain(map(ppm[PPM_CHANNEL_THROTTLE-1],PPM_MID+PPM_DEADBAND,PPM_MAX,0,1000), 0,1000);
}
int16_t getRCBrake() {
return constrain(map(ppm[PPM_CHANNEL_THROTTLE-1],PPM_MID-PPM_DEADBAND,PPM_MIN,0,1000), 0,1000);
}
//Return value -1000 - 1000
int16_t getRCSteer() {
if (ppm[PPM_CHANNEL_STEER-1]>PPM_MID+PPM_DEADBAND){
return constrain(map(ppm[PPM_CHANNEL_STEER-1],PPM_MID+PPM_DEADBAND,PPM_MAX,0,1000), 0,1000);
}else if (ppm[PPM_CHANNEL_STEER-1]<PPM_MID-PPM_DEADBAND){
return constrain(map(ppm[PPM_CHANNEL_STEER-1],PPM_MID-PPM_DEADBAND,PPM_MIN,0,-1000), -1000,0);
}else{
return 0;
}
}
//Return descrete value for equidistant ranges. modes = number of ranges.
uint8_t getRCMode(uint8_t modes) {
float v=constrain(map(ppm[PPM_CHANNEL_MODE-1],PPM_MIN,PPM_MAX,0.0,1.0), 0.0,0.999);
return uint8_t(v*modes);
}
void printPPM(unsigned long loopmillis){
count = count + 1;
Serial.print("ms :");
Serial.print(count);
Serial.print(" : ");
for (int i=1; i <= PPM_CHANNELS; i++) {
Serial.print(ppm[i-1]);
Serial.print(" ");
}
Serial.println();
}
#endif