working with nippleremote
This commit is contained in:
parent
e721d1e650
commit
a4e7c3e33d
339
controller.ino
339
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
|
||||
//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;
|
||||
}
|
||||
poti1=(poti1-POTIMIN)*1000/(POTIMAX-POTIMIN);
|
||||
//parse commands
|
||||
motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
|
||||
if (!motorenabled){ //disable motors?
|
||||
armed=false;
|
||||
Serial.println("!motorenebled. armed=false");
|
||||
}
|
||||
|
||||
poti2=analogRead(PIN_POTI2);
|
||||
if (poti2<POTIMIN){
|
||||
poti2=POTIMIN;
|
||||
}else if(poti2>POTIMAX){
|
||||
poti2=POTIMAX;
|
||||
}
|
||||
poti2=(poti2-POTIMIN)*1000/(POTIMAX-POTIMIN);
|
||||
|
||||
out_speed=poti1;
|
||||
out_steer=0;
|
||||
*/
|
||||
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");
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
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
|
||||
//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_speed=0;
|
||||
}
|
||||
|
||||
|
||||
if (flag_ppmupdated){
|
||||
flag_ppmupdated=false; //clear flag
|
||||
digitalWrite(PIN_LED,!digitalRead(PIN_LED)); //toggle led
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (millis()-last_send>SENDPERIOD){
|
||||
Serial2.write((uint8_t *) &out_steer, sizeof(out_steer));
|
||||
//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();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue