working with nippleremote
This commit is contained in:
parent
e721d1e650
commit
a4e7c3e33d
331
controller.ino
331
controller.ino
|
|
@ -7,61 +7,67 @@
|
||||||
//to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately
|
//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
|
//set boot0 back to 0 to run program on powerup
|
||||||
|
|
||||||
//#define DEBUG
|
#define DEBUG
|
||||||
|
|
||||||
#define PIN_LED PC13
|
#define PIN_LED PC13
|
||||||
|
|
||||||
#define PIN_POTI1 PA7
|
|
||||||
#define PIN_POTI2 PA6
|
|
||||||
|
|
||||||
#define SENDPERIOD 20
|
#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;
|
long last_send=0;
|
||||||
|
|
||||||
int16_t out_steer=0;
|
int16_t out_steer=0; //between -1000 and 1000
|
||||||
int16_t out_speed=0;
|
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() {
|
void setup() {
|
||||||
|
|
@ -72,73 +78,111 @@ void setup() {
|
||||||
|
|
||||||
|
|
||||||
pinMode(PIN_LED, OUTPUT);
|
pinMode(PIN_LED, OUTPUT);
|
||||||
digitalWrite(PIN_LED,LOW);
|
digitalWrite(PIN_LED,HIGH);
|
||||||
|
|
||||||
pinMode(PIN_POTI1, INPUT);
|
|
||||||
pinMode(PIN_POTI2, INPUT);
|
|
||||||
|
|
||||||
|
|
||||||
pinMode(PIN_CH1, INPUT);
|
Serial.println("Initializing nrf24");
|
||||||
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);
|
radio.begin();
|
||||||
attachInterrupt(PIN_CH2, ppmchanged2, CHANGE);
|
|
||||||
pinMode(PIN_CH3, INPUT);
|
|
||||||
attachInterrupt(PIN_CH3, ppmchanged3, CHANGE);
|
radio.setRetries(15,15); // optionally, increase the delay between retries & # of retries
|
||||||
pinMode(PIN_CH4, INPUT);
|
radio.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability
|
||||||
attachInterrupt(PIN_CH4, ppmchanged4, CHANGE);
|
|
||||||
|
radio.openWritingPipe(pipes[0]); //write on pipe 0
|
||||||
|
radio.openReadingPipe(1,pipes[1]); //read on pipe 1
|
||||||
|
|
||||||
|
radio.startListening();
|
||||||
|
|
||||||
|
Serial.println("Initialized");
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop() {
|
void loop() {
|
||||||
|
|
||||||
updateChannels(); //calculate chx_in times from ppm signals. 0 <= chx_in <= 1000
|
//NRF24
|
||||||
boolean _ppmOK=ppmOK();
|
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
|
//parse commands
|
||||||
/*poti1=analogRead(PIN_POTI1);
|
motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
|
||||||
#define POTIMIN 100
|
if (!motorenabled){ //disable motors?
|
||||||
#define POTIMAX 4000
|
armed=false;
|
||||||
if (poti1<POTIMIN){
|
Serial.println("!motorenebled. armed=false");
|
||||||
poti1=POTIMIN;
|
|
||||||
}else if(poti1>POTIMAX){
|
|
||||||
poti1=POTIMAX;
|
|
||||||
}
|
}
|
||||||
poti1=(poti1-POTIMIN)*1000/(POTIMAX-POTIMIN);
|
|
||||||
|
|
||||||
poti2=analogRead(PIN_POTI2);
|
if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==NRFDATA_CENTER){ //arm only when centered
|
||||||
if (poti2<POTIMIN){
|
armed=true; //arm at first received packet
|
||||||
poti2=POTIMIN;
|
Serial.println("centered. armed=true");
|
||||||
}else if(poti2>POTIMAX){
|
|
||||||
poti2=POTIMAX;
|
|
||||||
}
|
}
|
||||||
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;
|
out_steer=0;
|
||||||
*/
|
out_speed=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
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (flag_ppmupdated){
|
|
||||||
flag_ppmupdated=false; //clear flag
|
|
||||||
digitalWrite(PIN_LED,!digitalRead(PIN_LED)); //toggle led
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
if (millis()-last_send>SENDPERIOD){
|
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_steer, sizeof(out_steer));
|
||||||
Serial2.write((uint8_t *) &out_speed, sizeof(out_speed));
|
Serial2.write((uint8_t *) &out_speed, sizeof(out_speed));
|
||||||
|
Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));
|
||||||
last_send=millis();
|
last_send=millis();
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -147,118 +191,11 @@ void loop() {
|
||||||
Serial.print(out_steer);
|
Serial.print(out_steer);
|
||||||
Serial.print(" speed=");
|
Serial.print(" speed=");
|
||||||
Serial.print(out_speed);
|
Serial.print(out_speed);
|
||||||
/*
|
Serial.print(" checksum=");
|
||||||
Serial.print(ch1_in);
|
Serial.print(out_checksum);
|
||||||
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.println();
|
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();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue