Commits

Daniele Zambelli committed fd0eb49

fhl versione 120426

Comments (0)

Files changed (1)

 void loop() {
 //  if (state != UNKNOWN)
     readSensors();
-  print_on_led();
+  print_on_led(sensors);
   switch (state) {
     case START:        do_start();        break;
     case CENTER_L_INI: do_center_l_ini(); break;
     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;
-    default: {}
+    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();
+//      motors_stop();
 //      unknown();
 //      stop_you();
     break;
     state = new_state;
 }
 
-void unknown(){
+void unknown(int leds){
     set_motor(MOTOR_L, SP_ZERO);  // sostituire queste due righe con
     set_motor(MOTOR_R, SP_ZERO);  // motors_stop()
-    delay(1000);
+    print_on_led(leds);
+    delay(3000);
     state = UNKNOWN;
 }
 
 void do_start(){
-  if (sensors & B001100)           jump(SP_FULL, SP_FULL, STRAIGHT);
+  if (! (sensors & B110011))       jump(SP_FULL, SP_FULL, STRAIGHT);
   else if (sensors & B110000)      jump(SP_ZERO, SP_FULL, CENTER_L_INI);
   else if (sensors & B000011)      jump(SP_FULL, SP_ZERO, CENTER_R_INI);
-//  else                             unknown();
+  else                             unknown(B010101);
 }
 
-void do_center_l_ini(){
+void do_center_l_ini(){ 
   if (sensors & B000110)           jump(SP_FULL, SP_ZERO, CENTER_END);
 }
 
 }
 
 void do_straight(){
-  if (sensors == B000000)          unknown();
-  else if (sensors == B111111)     unknown();
+  if (sensors == B000000)          unknown(sensors);
+  else if (sensors == B111111)     unknown(sensors);
   else if (! (sensors & B110011))  {}
-  else if (sensors & B100000)      unknown(); //jump(SP_ZERO, SP_FULL, TURN_L_INI);
-  else if (sensors & B000001)      unknown(); //jump(SP_FULL, SP_ZERO, TURN_R_INI);
+  else 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 & B010000)      jump(SP_ADJUST, SP_FULL, ADJUST_L_INI);
   else if (sensors & B000010)      jump(SP_FULL, SP_ADJUST, ADJUST_R_INI);
-  else                             unknown();
+  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 == B001100)      jump(SP_FULL, SP_FULL, STRAIGHT);
+}
 
 void do_adj_l_ini(){
-  if (sensors & B100000)          unknown(); //jump(SP_ZERO, SP_FULL, TURN_L_INI);
-  else if (sensors & B000001)     unknown(); //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 & B000010)     jump(SP_FULL, SP_ADJUST, ADJUST_END);
 }
 
 void do_adj_r_ini(){
-  if (sensors & B100000)          unknown(); //jump(SP_ZERO, SP_FULL, TURN_L_INI);
-  else if (sensors & B000001)     unknown(); //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 & B010000)     jump(SP_ADJUST, SP_FULL, ADJUST_END);
 }
 
 void do_adj_end(){
-  if (sensors & B100000)          unknown(); //jump(SP_ZERO, SP_FULL, TURN_L_INI);
-  else if (sensors & B000001)     unknown(); //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 == B001100)    jump(SP_FULL, SP_FULL, STRAIGHT);
 }
 
-void print_on_led() { // print sensors state on leds
+void print_on_led(int leds) { // print sensors state on leds
   for (int i = 0; i < 6; i++)
 //    digitalWrite(PIN_LED[i], (((1<<i) & sensors) != 0) ? HIGH: LOW);
-    digitalWrite(PIN_LED[i], (1<<i) & sensors);
+    digitalWrite(PIN_LED[i], (1<<i) & leds);
 }
 
 void readSensors() {
   digitalWrite(MOTOR_STBY, LOW);
 }
 
-void set_motor(int motor, int speed) { // imposta la velocità tra -100 (indietro) e +100 (avanti)
-  if (speed){
-    motor_speed(motor, speed>0, map(abs(speed),0,100,50,255));}
+void set_motor(int motor, int sp) { // imposta la velocità tra -100 (indietro) e +100 (avanti)
+  if (sp){
+    motor_speed(motor, sp>0, map(abs(sp),0,100,50,255));}
   else {
-//    motor_coast(motor);
-    motors_stop();}
+    motor_speed(motor, 1, 0);}
 }
 
-void motor_speed(int motor, boolean direction, int speed) { // imposta la velocità tra 0 e 255
+void motor_speed(int motor, int dir, int sp) { // imposta la velocità tra 0 e 255
   if (motor == MOTOR_L) {
-    digitalWrite(MOTOR_LEFT_IN1, direction);
-    digitalWrite(MOTOR_LEFT_IN2, !direction);
-    analogWrite(MOTOR_LEFT_PWM, speed);}
+    digitalWrite(MOTOR_LEFT_IN1, dir);
+    digitalWrite(MOTOR_LEFT_IN2, !dir);
+    analogWrite(MOTOR_LEFT_PWM, sp);}
   else {
-    digitalWrite(MOTOR_RIGHT_IN1, direction);
-    digitalWrite(MOTOR_RIGHT_IN2, !direction);
-    analogWrite(MOTOR_RIGHT_PWM, speed);}
+    digitalWrite(MOTOR_RIGHT_IN1, dir);
+    digitalWrite(MOTOR_RIGHT_IN2, !dir);
+    analogWrite(MOTOR_RIGHT_PWM, sp);}
 }
 /* da togliere ???
 void motor_coast(boolean motor) { // motore in folle