|
@@ -65,24 +65,40 @@ void loop() {
|
|
|
|
|
|
//Refresh and scale raw move values
|
|
//Refresh and scale raw move values
|
|
move = map(ch2, 1000,2000, -255, 255); //center over zero
|
|
move = map(ch2, 1000,2000, -255, 255); //center over zero
|
|
-
|
|
|
|
|
|
+ move = constrain(move, -255, 255);
|
|
// turn = map(ch1,1000,2000,-255,255);
|
|
// turn = map(ch1,1000,2000,-255,255);
|
|
// turn = constrain(turn, -255, 255);
|
|
// turn = constrain(turn, -255, 255);
|
|
turn = map(ch1,1000,2000,-255,255);
|
|
turn = map(ch1,1000,2000,-255,255);
|
|
- turn = constrain(turn, -move, move);
|
|
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ Serial.print("move:"); //Serial debugging stuff
|
|
|
|
+ Serial.println(move);
|
|
|
|
+
|
|
|
|
+ Serial.print("turn:"); //Serial debugging stuff
|
|
|
|
+ Serial.println(turn);
|
|
|
|
|
|
//calculate each motor speed
|
|
//calculate each motor speed
|
|
int speeds[2] = {0, 0} ;
|
|
int speeds[2] = {0, 0} ;
|
|
- int dirs[2] = {0, 0} ;
|
|
|
|
|
|
+// int dirs[2] = {0, 0} ;
|
|
|
|
+
|
|
|
|
+ int Dir = move>0 ? 1 : 0;
|
|
|
|
+ move = abs(move) ;
|
|
|
|
+ turn = constrain(turn, -move, move);
|
|
|
|
+ if (turn>0) { speeds[0] = move; speeds[1] = move - turn ; }
|
|
|
|
+ if (turn<=0) { speeds[0] = move + turn ; speeds[1] = move ;
|
|
|
|
+ }
|
|
|
|
+// speeds[1] = move ;
|
|
|
|
+// speeds[0] = move ;
|
|
|
|
+ Serial.print("speeds: "); //Serial debugging stuff
|
|
|
|
+ Serial.print(speeds[0]);
|
|
|
|
+ Serial.print(" - ");
|
|
|
|
+ Serial.println(speeds[1]);
|
|
|
|
|
|
- if (turn>0) { speeds[0] = move; speeds[1] = move - turn; }
|
|
|
|
- else {turn=abs(turn); speeds[0] = move-turn; speeds[1] = move ;}
|
|
|
|
-
|
|
|
|
//apply to driver
|
|
//apply to driver
|
|
for(int i =0; i<2; i++) {
|
|
for(int i =0; i<2; i++) {
|
|
- dirs[i] = speeds[i] > 0 ? 1 : 0 ;
|
|
|
|
|
|
+// dirs[i] = speeds[i] > 0 ? 1 : 0 ;
|
|
speeds[i] = abs( speeds[i]);
|
|
speeds[i] = abs( speeds[i]);
|
|
- digitalWrite(dirs_pin[i], dirs[i]) ;
|
|
|
|
|
|
+ digitalWrite(dirs_pin[i], Dir) ;
|
|
analogWrite(speeds_pin[i], speeds[i]) ;
|
|
analogWrite(speeds_pin[i], speeds[i]) ;
|
|
|
|
|
|
}
|
|
}
|
|
@@ -97,18 +113,21 @@ void loop() {
|
|
// if(turn<0){turn=abs(turn); analogWrite(pwm_a, move-turn); analogWrite(pwm_b, move);};
|
|
// if(turn<0){turn=abs(turn); analogWrite(pwm_a, move-turn); analogWrite(pwm_b, move);};
|
|
//// analogWrite(pwm_b, move); analogWrite(pwm_a, move);
|
|
//// analogWrite(pwm_b, move); analogWrite(pwm_a, move);
|
|
|
|
|
|
- Serial.print("move:"); //Serial debugging stuff
|
|
|
|
- Serial.println(move);
|
|
|
|
|
|
|
|
- Serial.print("turn:"); //Serial debugging stuff
|
|
|
|
- Serial.println(turn);
|
|
|
|
|
|
|
|
- Serial.print("move-turn:"); //Serial debugging stuff
|
|
|
|
- Serial.println(move-turn);
|
|
|
|
|
|
+ Serial.print("A: "); //Serial debugging stuff
|
|
|
|
+ Serial.print(speeds[0]);
|
|
|
|
+ Serial.print(" - ");
|
|
|
|
+// Serial.println(dirs[0]);
|
|
|
|
+
|
|
|
|
+ Serial.print("B: "); //Serial debugging stuff
|
|
|
|
+ Serial.print(speeds[1]);
|
|
|
|
+ Serial.print(" - ");
|
|
|
|
+// Serial.println(dirs[1]);
|
|
|
|
|
|
Serial.println(); //Serial debugging stuff
|
|
Serial.println(); //Serial debugging stuff
|
|
Serial.println();
|
|
Serial.println();
|
|
- Serial.println();
|
|
|
|
|
|
+// Serial.println();
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|