diff --git a/nippleremote_firmware/lib/RF24 b/nippleremote_firmware/lib/RF24 index ebcd0d1..b18ed8c 160000 --- a/nippleremote_firmware/lib/RF24 +++ b/nippleremote_firmware/lib/RF24 @@ -1 +1 @@ -Subproject commit ebcd0d1d0b3061fcb57444e1dbe5829ef25705cd +Subproject commit b18ed8c0133e1d1502cb52ce723531f611252763 diff --git a/nippleremote_firmware/src/main.cpp b/nippleremote_firmware/src/main.cpp index 559a0b1..81ed10b 100644 --- a/nippleremote_firmware/src/main.cpp +++ b/nippleremote_firmware/src/main.cpp @@ -55,7 +55,7 @@ const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL }; //0xF0F0F0F0xxLL, struct nrfdata { uint8_t steer; //between 0 and 255, 127 is stop. will be scaled to -1000 to 1000 uint8_t speed; //between 0 and 255, 127 is stop. will be scaled to -1000 to 1000 - uint8_t commands; //bit 0 set = motor enable + uint8_t commands; //bit 0 set = motor enable, 1=left,2=up,3=down,4=right uint8_t checksum; }; @@ -77,17 +77,11 @@ boolean motorenabled=false; #define TRACKPOINT_MAX 70 //value for maximum stick movement #define TRACKPOINT_MAX_ERROR 90 //if value above this amount, error is triggered (nipple position stuck. maybe fixed by slowing down reading interval) -float speedscale=0.0; -float steerscale=0.0; +float speedscale=1.0; +float steerscale=1.0; int16_t last_xin=0; int16_t last_yin=0; -int16_t xin_smooth=0; -int16_t yin_smooth=0; -int16_t maxacc=0; -int16_t maxacc_brake=0; -int16_t maxaccsteer=0; -int16_t maxaccsteer_brake=0; #define SETUP_NONE 0 #define SETUP_WAIT 1 //waiting for input @@ -98,8 +92,7 @@ long setupmode_waitstarttime=0; //starttime of SETUP_WAIT mode #define SETUP_HOLD_POWEROFF 2000 //if button held down after x ms in setup mode, power off #define SETUP_DONE_TIME 1000 //time to keep motors disabled after exiting setup #define SETUP_MOVE_THRESHOLD 275 //500*TRACKPOINT_MAX/127 -uint8_t speedmode=1; //0 (slow), 1(medium), 2(fast) -#define SETUP_SPEEDMODE_MAX 2 + uint16_t led_ton=0; //never. time in ms for on time uint16_t led_toff=65535; //always @@ -111,6 +104,8 @@ long time_lastactivity=0; boolean touching=false; +uint8_t setup_directon_press=0; // 1=left,2=up,3=down,4=right + int voltage=4000; #define VOLTAGE_WARN 3400 /* @@ -119,7 +114,7 @@ int voltage=4000; void sendRF(nrfdata senddata); long readVcc(); -void setup_updateSpeedmode(); + unsigned long last_testprint=0; @@ -179,7 +174,6 @@ void setup() { Serial.print("Voltage="); Serial.println( voltage, DEC ); - setup_updateSpeedmode(); //set speeds } @@ -218,13 +212,12 @@ void loop() { Serial.println(d.y); #endif - Serial.print("DataReport: "); - Serial.print(d.x); - Serial.print(", "); - Serial.println(d.y); nrfdata senddata; + + + //senddata.steer=map(constrain((uint8_t)(d.x+127),127-TRACKPOINT_MAX,127+TRACKPOINT_MAX) , 127-TRACKPOINT_MAX,127+TRACKPOINT_MAX, 127+(127*steerscale), 127-(127*steerscale) ); //steer //senddata.speed=map(constrain((uint8_t)(d.y+127),127-TRACKPOINT_MAX,127+TRACKPOINT_MAX) , 127-TRACKPOINT_MAX,127+TRACKPOINT_MAX, 127-(127*speedscale), 127+(127*speedscale) ); //speed @@ -281,53 +274,9 @@ void loop() { Serial.print(", "); Serial.println(phi,4);*/ - - //xin_smooth=smoothfilter*xin_smooth + (1-smoothfilter)*xin; - //yin_smooth=smoothfilter*yin_smooth + (1-smoothfilter)*yin; - if (maxaccsteer>0){ - // ### X ### - int16_t _xaccel=xin_smooth-xin; - if ((xin_smooth>0 && xin<-2) || (xin_smooth<0 && xin>2) ){ //if actively braking - if (_xaccel<-maxaccsteer_brake){ //limit deceleration - _xaccel=-maxaccsteer_brake; - }else if (_xaccel>maxaccsteer_brake){ - _xaccel=maxaccsteer_brake; - } - }else{ //not braking - if (_xaccel<-maxaccsteer){ //limit acceleration - _xaccel=-maxaccsteer; - }else if (_xaccel>maxaccsteer){ - _xaccel=maxaccsteer; - } - } - xin_smooth-=_xaccel; //update value - }else{ //no acc limit - xin_smooth=xin; //update immediately - } - if (maxacc>0){ - // ### Y ### - int16_t _yaccel=yin_smooth-yin; - if ((yin_smooth>0 && yin<-2) || (yin_smooth<0 && yin>2) ){ //if actively braking - if (_yaccel<-maxacc_brake){ //limit deceleration - _yaccel=-maxacc_brake; - }else if (_yaccel>maxacc_brake){ - _yaccel=maxacc_brake; - } - }else{ //not braking - if (_yaccel<-maxacc){ //limit acceleration - _yaccel=-maxacc; - }else if (_yaccel>maxacc){ - _yaccel=maxacc; - } - } - yin_smooth-=_yaccel; //update value - }else{ //no acc limit - yin_smooth=yin; - } - - senddata.steer=map(xin_smooth, -1000,1000, 127+(128*steerscale), 127-(127*steerscale) ); //steer - senddata.speed=map(yin_smooth, -1000,1000, 127-(127*speedscale), 127+(128*speedscale) ); //speed + senddata.steer=map(xin, -1000,1000, 127+(128*steerscale), 127-(127*steerscale) ); //steer + senddata.speed=map(yin, -1000,1000, 127-(127*speedscale), 127+(128*speedscale) ); //speed senddata.commands=0; //reset @@ -335,11 +284,15 @@ void loop() { //senddata.steer=127; //stop //senddata.speed=127; senddata.commands|= 0 << 0; //motorenabled send false - xin_smooth=0; //reset smooth value - yin_smooth=0; + }else{ senddata.commands|= motorenabled << 0; //motorenabled is bit 0 } + + if (setup_directon_press!=0) { + senddata.commands|= 1 << setup_directon_press; //motorenabled is bit 0 + setup_directon_press=0; //reset after sending + } #ifdef DEBUG Serial.print(senddata.steer); @@ -403,35 +356,23 @@ void loop() { if (millis()>setupmode_waitstarttime+SETUP_WAIT_TIMEOUT){ //waittime over setupmode=SETUP_NONE; //exit setup }else if(last_yin > SETUP_MOVE_THRESHOLD){ //y moved up - Serial.print("Moved Up"); - if (speedmode0){ //if not already at minimum - speedmode-=1; - } - setup_updateSpeedmode(); + Serial.println("Moved Down"); + setup_directon_press=3; setupmode=SETUP_DONE; //exit setupmode setupmode_waitstarttime=millis();//use this value for done timer }else if(last_xin < -SETUP_MOVE_THRESHOLD){ //y moved left - Serial.print("Moved Left"); - maxacc=20; //the higher the snappier - maxacc_brake=30; - maxaccsteer=30; - maxaccsteer_brake=70; + Serial.println("Moved Left"); + setup_directon_press=1; setupmode=SETUP_DONE; //exit setupmode setupmode_waitstarttime=millis();//use this value for done timer }else if(last_xin > SETUP_MOVE_THRESHOLD){ //y moved right - Serial.print("Moved Right"); - maxacc=0; - maxacc_brake=0; - maxaccsteer=0; - maxaccsteer_brake=0; + Serial.println("Moved Right"); + setup_directon_press=4; setupmode=SETUP_DONE; //exit setupmode setupmode_waitstarttime=millis();//use this value for done timer }else if (millis()>setupmode_waitstarttime+SETUP_HOLD_POWEROFF && !digitalRead(PIN_BUTTON)){ //if button held down after SETUP_HOLD_POWEROFF @@ -549,24 +490,3 @@ long readVcc() { result = 1126400L / result; // Back-calculate AVcc in mV return result; } - -void setup_updateSpeedmode(){ - switch(speedmode){ - case 0: //slow - speedscale=0.15; - steerscale=0.2; - break; - case 1: //medium - speedscale=0.4; - steerscale=0.45; - break; - case 2: //fast - speedscale=1.0; - steerscale=0.8; - break; - default: - speedscale=0.1; - steerscale=0.1; - break; - } -} \ No newline at end of file