remove mode logic

nippleremote_firmware/lib/RF24

@@ -1 +1 @@
-Subproject commit ebcd0d1d0b3061fcb57444e1dbe5829ef25705cd
+Subproject commit b18ed8c0133e1d1502cb52ce723531f611252763

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 speedscale=1.0;
-float steerscale=0.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
+    senddata.steer=map(xin, -1000,1000, 127+(128*steerscale), 127-(127*steerscale) ); //steer
-      xin_smooth=xin; //update immediately
+    senddata.speed=map(yin, -1000,1000, 127-(127*speedscale), 127+(128*speedscale) ); //speed
-    }
-    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.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");
+        Serial.println("Moved Up");
-        if (speedmode<SETUP_SPEEDMODE_MAX){ //if not already at maximum
+        setup_directon_press=2;
-          speedmode+=1;
-        }
-        setup_updateSpeedmode();
         setupmode=SETUP_DONE; //exit setupmode
         setupmode_waitstarttime=millis(); //use this value for done timer
       }else if(last_yin < -SETUP_MOVE_THRESHOLD){ //y moved down
-        Serial.print("Moved Down");
+        Serial.println("Moved Down");
-        if (speedmode>0){ //if not already at minimum
+        setup_directon_press=3;
-          speedmode-=1;
-        }
-        setup_updateSpeedmode();
         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");
+        Serial.println("Moved Left");
-        maxacc=20; //the higher the snappier
+        setup_directon_press=1;
-        maxacc_brake=30;
-        maxaccsteer=30;
-        maxaccsteer_brake=70;
         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");
+        Serial.println("Moved Right");
-        maxacc=0;
+        setup_directon_press=4;
-        maxacc_brake=0;
-        maxaccsteer=0;
-        maxaccsteer_brake=0;
         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;
-  }
-}