innovationBot/BehaviourController.h at master · panGenerator/innovationBot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84


class BehaviourController {

  public:

  BotBehaviour previousBehaviour = NULL;
  BotBehaviour currentBehaviour = WAITING;

  MotorController* motorController;
  SensorsController* sensorsController;

  void setup( MotorController* _motorCtrl, SensorsController* _sensorsCtrl ){
    motorController = _motorCtrl;
    sensorsController = _sensorsCtrl;
  }

  void setBehaviour( BotBehaviour newBehaviour ){

    currentBehaviour = newBehaviour;

    switch( newBehaviour ){

      case WAITING:
        Serial.println("WAITING");
      break;

      case MOVING_FORWARD:
        Serial.println("MOVING_FORWARD");
        motorController->moveStraight( random( 1000, 3000 ) , random( 100, 250 ), FORWARD );
      break;

      case RETREAT:
        Serial.println("RETREAT");
        motorController->moveStraight( random( 1000, 1500 ) , random( 150 ), BACKWARD );
      break;

      case LOOKING_AROUND:
        Serial.println("LOOKING_AROUND");
        motorController->turn( random( 500, 1000 ), 150 , ( random(0,10) > 5 ) ? LEFT : RIGHT );
      break;

    }

    previousBehaviour = currentBehaviour;

  }

  void update(){

    if( sensorsController->isFrontSensorTriggered() ){

      Serial.println( "FRONT SENSOR TRIGGERED!" );
      Serial.println( sensorsController->getFrontSensorValue() );

      if( currentBehaviour != RETREAT && currentBehaviour != LOOKING_AROUND ){
        setBehaviour( RETREAT );
      }

    }

    if( sensorsController->isFloorSensorTriggered() ){
      Serial.println( "FLOOR SENSOR TRIGGERED!" );
      setBehaviour( RETREAT );
    }

    if( motorController->isMoveFinished() ){

      if( currentBehaviour == MOVING_FORWARD || currentBehaviour == RETREAT ){

        setBehaviour( LOOKING_AROUND );

      }else if( currentBehaviour == LOOKING_AROUND ){

        setBehaviour( MOVING_FORWARD );

      }


    }

  }

};