Commits

Daniele Zambelli committed a21c7d2

fhl versione 120502

Comments (0)

Files changed (1)

 //////
 // fisical values
 #define SP_ZERO                0
-#define SP_FULL              170     // from 0 to 255
-#define ADJUST                90
-#define DAMPING                 .1
+#define SP_FULL              150     // from 0 to 255
+#define ADJUST                50
+#define DAMPING                 .2
 //#define SP_HALF               25
 //#define SP_ADJUST             25
 #define THRESHOLD     	     200     // soglia di riconosc. bianco/nero
     case ADJUST_L_INI: do_adj_l_ini();    break;
     case ADJUST_R_INI: do_adj_r_ini();    break;
     case ADJUST_END:   do_adj_end();      break;
-//    case TURN_L_INI:   do_turn_l_ini();   break;
-//    case TURN_R_INI:   do_turn_r_ini();   break;
-//    case TURN_END:     do_turn_end();     break;
+    case TURN_L_INI:   do_turn_l_ini();   break;
+    case TURN_R_INI:   do_turn_r_ini();   break;
+    case TURN_END:     do_turn_end();     break;
 //    default: motors_stop();
 //      motor_activation(false);
 //      motors_stop();
   if (sensors == B000000)          unknown(sensors);
   else if (sensors == B111111)     unknown(sensors);
   else if (! (sensors & B110011))  damping();
-  else if (sensors & B100000)      unknown(sensors);// jump(SP_ZERO, SP_FULL, TURN_L_INI);
-  else if (sensors & B000001)      unknown(sensors);// jump(SP_FULL, SP_ZERO, TURN_R_INI);
+  else if (sensors & B100000)      jump(-SP_FULL, SP_ZERO, TURN_L_INI);
+  else if (sensors & B000001)      jump(SP_ZERO, -SP_FULL, TURN_R_INI);
   else if (sensors & B010000)      
     jump(SP_FULL-ADJUST, SP_FULL+ADJUST, ADJUST_L_INI);
   else if (sensors & B000010)
   else                             unknown(sensors);
 }
 
-//void do_turn_l_ini(){
-//  if (sensors & B000010)       jump(SP_FULL, SP_ZERO, TURN_END);
-//}
-//
-//void do_turn_r_ini(){
-//  if (sensors & B010000)       jump(SP_ZERO, SP_FULL, TURN_END);
-//}
-//
-//void do_turn_end(){              // same as do_center_end()
-//  if (sensors == B001100)      jump(SP_FULL, SP_FULL, STRAIGHT);
-//}
+void do_turn_l_ini(){
+  if (sensors & B000010)       jump(SP_FULL, SP_ZERO, TURN_END);
+}
+
+void do_turn_r_ini(){
+  if (sensors & B010000)       jump(SP_ZERO, SP_FULL, TURN_END);
+}
+
+void do_turn_end(){              // same as do_center_end()
+  if ((sensors & B001000) || (sensors & B000100))
+    unknown(sensors); //jump(SP_FULL, SP_FULL, STRAIGHT);
+}
 
 void do_adj_l_ini(){
-  if (sensors & B100000)          unknown(B100001);// jump(SP_ZERO, SP_FULL, TURN_L_INI);
-  else if (sensors & B000001)     unknown(B100001);// jump(SP_FULL, SP_ZERO, TURN_R_INI);
+  if (sensors & B100000)          jump(SP_ZERO, SP_FULL, TURN_L_INI);
+  else if (sensors & B000001)     jump(SP_FULL, SP_ZERO, TURN_R_INI);
   else if (sensors & B01000)      damping();
   else if (!(sensors & B01000))   jump(speed_r, speed_l, STRAIGHT);
+//  else if (!(sensors & B01000))   jump(SP_FULL-ADJUST*.5, SP_FULL+ADJUST*.5, STRAIGHT);
 //  else if (!(sensors & B01000))   jump(SP_FULL, SP_FULL*.8, ADJUST_END);
 }
 
 void do_adj_r_ini(){
-  if (sensors & B100000)          unknown(B100001);// jump(SP_ZERO, SP_FULL, TURN_L_INI);
-  else if (sensors & B000001)     unknown(B100001);// jump(SP_FULL, SP_ZERO, TURN_R_INI);
+  if (sensors & B100000)          jump(SP_ZERO, SP_FULL, TURN_L_INI);
+  else if (sensors & B000001)     jump(SP_FULL, SP_ZERO, TURN_R_INI);
   else if (sensors & B00010)      damping();
-  else if (!(sensors & B00010))   jump(speed_r, speed_l, STRAIGHT);
+  else if (!(sensors & B000010))   jump(speed_r, speed_l, STRAIGHT);
+//  else if (!(sensors & B00010))   jump(SP_FULL+ADJUST*.5, SP_FULL-ADJUST*.5, STRAIGHT);
 //  else if (!(sensors & B000010))  jump(SP_FULL*.8, SP_FULL, ADJUST_END);
 }
 
   int dir = int (sp_l >= 0);
   digitalWrite(MOTOR_LEFT_IN1, dir);
   digitalWrite(MOTOR_LEFT_IN2, !dir);
-  analogWrite(MOTOR_LEFT_PWM, sp_l); //map(abs(sp_l),0,100,0,255));
+  analogWrite(MOTOR_LEFT_PWM, abs(sp_l)); //map(abs(sp_l),0,100,0,255));
   dir = int (sp_r >= 0);
   digitalWrite(MOTOR_RIGHT_IN1, dir);
   digitalWrite(MOTOR_RIGHT_IN2, !dir);
-  analogWrite(MOTOR_RIGHT_PWM, sp_r); //map(abs(sp_r),0,100,0,255));
+  analogWrite(MOTOR_RIGHT_PWM, abs(sp_r)); //map(abs(sp_r),0,100,0,255));
 }
 
 /* da togliere ???