diff --git a/controller.ino b/controller.ino index 5801ad4..c8f9166 100644 --- a/controller.ino +++ b/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 +#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 (poti1POTIMAX){ - 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 (poti2POTIMAX){ - 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(); - } -}