working with nippleremote

This commit is contained in:
interfisch 2019-03-16 20:02:27 +01:00
parent e721d1e650
commit a4e7c3e33d
1 changed files with 138 additions and 201 deletions

View File

@ -7,61 +7,67 @@
//to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately
//set boot0 back to 0 to run program on powerup
//#define DEBUG
#define DEBUG
#define PIN_LED PC13
#define PIN_POTI1 PA7
#define PIN_POTI2 PA6
#define SENDPERIOD 20
uint16_t poti1=0;
uint16_t poti2=0;
//from left to right. pins at bottom. chips on top
//1 GND (black)
//2 Data
//3 Clock
//4 Reset
//5 +5V (red)
//6 Right BTN
//7 Middle BTN
//8 Left BTN
//pinout: https://martin-prochnow.de/projects/thinkpad_keyboard
//see also https://github.com/feklee/usb-trackpoint/blob/master/code/code.ino
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
RF24 radio(PB0,PB1); //ce, cs
//SCK D13 (Pro mini), A5 (bluepill)
//Miso D12 (Pro mini), A6 (bluepill)
//Mosi D11 (Pro mini), A7 (bluepill)
// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
struct nrfdata {
uint8_t steer;
uint8_t speed;
uint8_t commands; //bit 0 set = motor enable
uint8_t checksum;
};
nrfdata lastnrfdata;
long last_nrfreceive=0; //last time values were received and checksum ok
long nrf_delay=0;
#define MAX_NRFDELAY 50
//command variables
boolean motorenabled=false; //set by nrfdata.commands
long last_send=0;
int16_t out_steer=0;
int16_t out_steer=0; //between -1000 and 1000
int16_t out_speed=0;
uint8_t out_checksum=0; //0= disable motors, 255=reserved, 1<=checksum<255
#define NRFDATA_CENTER 127
boolean armed=false;
volatile long t_raising1=0;
volatile long t_falling1=0;
volatile long t_raising2=0;
volatile long t_falling2=0;
volatile long t_raising3=0;
volatile long t_falling3=0;
volatile long t_raising4=0;
volatile long t_falling4=0;
long last_updated_ch1=0;
long last_updated_ch2=0;
long last_updated_ch3=0;
long last_updated_ch4=0;
long ppmlagmillis=0; //updated by ppmOK()
boolean flag_ppmupdated=false; //true if at least one ppm value (chx_in) updated
#define MAXPPMUPDATETIME 50 //max time it should take to update a ppm channel value (otherwise ppmOK() will return false)
uint16_t ch1_in;
uint16_t ch2_in;
uint16_t ch3_in;
uint16_t ch4_in;
//ch1 = steer (ail)
//ch2 = speed (ele)
//ch3 = speed multiplier (thr)
#define PIN_CH1 PB9
#define PIN_CH2 PB8
#define PIN_CH3 PB7
#define PIN_CH4 PB6
#define PPM_TIME_MIN 900
#define PPM_TIME_MAX 2100
void setup() {
@ -72,193 +78,124 @@ void setup() {
pinMode(PIN_LED, OUTPUT);
digitalWrite(PIN_LED,LOW);
pinMode(PIN_POTI1, INPUT);
pinMode(PIN_POTI2, INPUT);
digitalWrite(PIN_LED,HIGH);
pinMode(PIN_CH1, INPUT);
attachInterrupt(PIN_CH1, ppmchanged1, CHANGE); //see http://docs.leaflabs.com/static.leaflabs.com/pub/leaflabs/maple-docs/0.0.12/lang/api/attachinterrupt.html
pinMode(PIN_CH2, INPUT);
attachInterrupt(PIN_CH2, ppmchanged2, CHANGE);
pinMode(PIN_CH3, INPUT);
attachInterrupt(PIN_CH3, ppmchanged3, CHANGE);
pinMode(PIN_CH4, INPUT);
attachInterrupt(PIN_CH4, ppmchanged4, CHANGE);
Serial.println("Initializing nrf24");
radio.begin();
radio.setRetries(15,15); // optionally, increase the delay between retries & # of retries
radio.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability
radio.openWritingPipe(pipes[0]); //write on pipe 0
radio.openReadingPipe(1,pipes[1]); //read on pipe 1
radio.startListening();
Serial.println("Initialized");
}
void loop() {
updateChannels(); //calculate chx_in times from ppm signals. 0 <= chx_in <= 1000
boolean _ppmOK=ppmOK();
//NRF24
nrf_delay=millis()-last_nrfreceive; //update nrf delay
if ( radio.available() )
{
//Serial.println("radio available ...");
bool done = false;
while (!done)
{
digitalWrite(PIN_LED, !digitalRead(PIN_LED));
done = radio.read( &lastnrfdata, sizeof(nrfdata) );
//Potis
/*poti1=analogRead(PIN_POTI1);
#define POTIMIN 100
#define POTIMAX 4000
if (poti1<POTIMIN){
poti1=POTIMIN;
}else if(poti1>POTIMAX){
poti1=POTIMAX;
//parse commands
motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
if (!motorenabled){ //disable motors?
armed=false;
Serial.println("!motorenebled. armed=false");
}
poti1=(poti1-POTIMIN)*1000/(POTIMAX-POTIMIN);
poti2=analogRead(PIN_POTI2);
if (poti2<POTIMIN){
poti2=POTIMIN;
}else if(poti2>POTIMAX){
poti2=POTIMAX;
if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==NRFDATA_CENTER){ //arm only when centered
armed=true; //arm at first received packet
Serial.println("centered. armed=true");
}
poti2=(poti2-POTIMIN)*1000/(POTIMAX-POTIMIN);
out_speed=poti1;
uint8_t calcchecksum=(uint8_t)((lastnrfdata.steer+3)*(lastnrfdata.speed+13));
if (lastnrfdata.checksum!=calcchecksum){ //checksum not ok?
armed=false;
Serial.println("Checksum fail. armed=false");
}else{ //checksum ok
last_nrfreceive=millis();
}
#ifdef DEBUG
Serial.print("Received:");
Serial.print(" st=");
Serial.print(lastnrfdata.steer);
Serial.print(", sp=");
Serial.print(lastnrfdata.speed);
Serial.print(", c=");
Serial.print(lastnrfdata.commands);
Serial.print(", chks=");
Serial.print(lastnrfdata.checksum);
Serial.print("nrfdelay=");
Serial.print(nrf_delay);
#endif
//y positive = forward
//x positive = right
}
}
if (armed && nrf_delay>=MAX_NRFDELAY){ //too long since last sucessful nrf receive
armed=false;
Serial.println("too long since last nrf data. armed=false");
}
if (armed){
//out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
//out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
out_speed=(int16_t)( ((int16_t)(lastnrfdata.speed)-NRFDATA_CENTER)*1000/127 );
out_steer=(int16_t)( ((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*1000/127 );
}else{ //took too long since last nrf data
out_steer=0;
*/
if (_ppmOK){ //ppm check failed
//out_speed=(int16_t)( (ch2_in*2-1000)*(ch3_in/1000.0) );
out_speed=(int16_t)( (ch2_in*2-1000));
out_steer=ch1_in*2-1000;
}else{
//out_speed=(int16_t)( (ch2_in*2-1000)*(ch3_in/1000.0) );
out_speed=0; //stop
out_steer=0; //stop
out_speed=0;
}
if (flag_ppmupdated){
flag_ppmupdated=false; //clear flag
digitalWrite(PIN_LED,!digitalRead(PIN_LED)); //toggle led
}
if (millis()-last_send>SENDPERIOD){
//calculate checksum
out_checksum = ((uint8_t) ((uint8_t)out_steer)*((uint8_t)out_speed)); //simple checksum
if (out_checksum==0 || out_checksum==255){ out_checksum=1; } //cannot be 0 or 255 (special purpose)
if (!motorenabled){ //disable motors?
out_checksum=0; //checksum=0 disables motors
}
Serial2.write((uint8_t *) &out_steer, sizeof(out_steer));
Serial2.write((uint8_t *) &out_speed, sizeof(out_speed));
Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));
last_send=millis();
#ifdef DEBUG
Serial.print("steer=");
Serial.print(" steer=");
Serial.print(out_steer);
Serial.print(" speed=");
Serial.print(out_speed);
/*
Serial.print(ch1_in);
Serial.print(",");
Serial.print(ch2_in);
Serial.print(",");
Serial.print(ch3_in);
Serial.print(",");
Serial.print(ch4_in);
*/
Serial.print(", ppmOK=");
Serial.print(_ppmOK);
Serial.print(", ppmlag=");
Serial.print(ppmlagmillis);
#endif
Serial.print(" checksum=");
Serial.print(out_checksum);
Serial.println();
#endif
}
}
void updateChannels(){
long funcmillis=millis();
//Calculate Pulse Width. (pulseIn values typically between 1000 and 2000). Map and constrain between 0 and 1000
long new_ch1_in=(t_falling1-t_raising1);
if (new_ch1_in>=PPM_TIME_MIN && new_ch1_in<=PPM_TIME_MAX){ //time valid and has changed
ch1_in=constrain(map(new_ch1_in,1000,2000, 0,1000), 0,1000);
last_updated_ch1=funcmillis;
flag_ppmupdated=true;
}
long new_ch2_in=(t_falling2-t_raising2);
if (new_ch2_in>=PPM_TIME_MIN && new_ch2_in<=PPM_TIME_MAX){ //time valid and has changed
ch2_in=constrain(map(new_ch2_in,1000,2000, 0,1000), 0,1000);
last_updated_ch2=funcmillis;
flag_ppmupdated=true;
}
long new_ch3_in=(t_falling3-t_raising3);
if (new_ch3_in>=PPM_TIME_MIN && new_ch3_in<=PPM_TIME_MAX){ //time valid and has changed
ch3_in=constrain(map(new_ch3_in,1000,2000, 0,1000), 0,1000);
last_updated_ch3=funcmillis;
flag_ppmupdated=true;
}
long new_ch4_in=(t_falling4-t_raising4);
if (new_ch4_in>=PPM_TIME_MIN && new_ch4_in<=PPM_TIME_MAX){ //time valid and has changed
ch4_in=constrain(map(new_ch4_in,1000,2000, 0,1000), 0,1000);
last_updated_ch4=funcmillis;
flag_ppmupdated=true;
}
}
boolean ppmOK(){
long m=millis();
boolean returnvalue=true;
ppmlagmillis=0;
long v;
v=m-last_updated_ch1;
if (v>ppmlagmillis){ ppmlagmillis=v; }
if (v>MAXPPMUPDATETIME){
returnvalue=false;
}
v=m-last_updated_ch2;
if (v>ppmlagmillis){ ppmlagmillis=v; }
if (v>MAXPPMUPDATETIME){
returnvalue=false;
}
v=m-last_updated_ch3;
if (v>ppmlagmillis){ ppmlagmillis=v; }
if (v>MAXPPMUPDATETIME){
returnvalue=false;
}
v=m-last_updated_ch4;
if (v>ppmlagmillis){ ppmlagmillis=v; }
if (v>MAXPPMUPDATETIME){
returnvalue=false;
}
return returnvalue;
}
void ppmchanged1(){
if (digitalRead(PIN_CH1)){
t_raising1=micros();
}else{
t_falling1=micros();
}
}
void ppmchanged2(){
if (digitalRead(PIN_CH2)){
t_raising2=micros();
}else{
t_falling2=micros();
}
}
void ppmchanged3(){
if (digitalRead(PIN_CH3)){
t_raising3=micros();
}else{
t_falling3=micros();
}
}
void ppmchanged4(){
if (digitalRead(PIN_CH4)){
t_raising4=micros();
}else{
t_falling4=micros();
}
}