MicroMouse/motors.cpp at main · M0hamed-Gamall/MicroMouse · 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
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
#include <Arduino.h>
#include "motors.h"
#include "sensors.h"


#define ENA 5  // Rigth motor
#define in1 10
#define in2 9

#define ENB 6  // Left motor
#define in3 12
#define in4 11

// start encoder   Pins //
// #define encoder1 7
#define encoder 8
// end encoder   Pins //


// extern float AngleZ;
float target_angle = 0;
// float gyroRatio =1.0 ;


int max_RightSpeed = 56;
int max_LeftSpeed = 60;
int curr_RightSpeed = 47;
int curr_LeftSpeed = 52;
int turn_RightSpeed = 40;
int turn_LeftSpeed = 40;

//// encoder  //////
int counter = 0;
int state = 0;
int last_state = 0;


float last_error = 0;
float kp = .4; // .4
float kd = 1;  // 1
float error = 0;
int change = 0;


void initializeMotors(){
  pinMode(ENA , OUTPUT);
  pinMode(ENB , OUTPUT);

  pinMode(in1 , OUTPUT);
  pinMode(in2 , OUTPUT);
  pinMode(in3 , OUTPUT);
  pinMode(in4 , OUTPUT);

  pinMode(encoder , INPUT);

}

void moveForward(){
  if(wallFront()){
    stop();
    return;
  }
	// to push motors
  digitalWrite(13 , HIGH);
  // delay(10);
  analogWrite(ENA , 90);       // Right motor
  digitalWrite(in1 , HIGH);
  digitalWrite(in2 , LOW);

  analogWrite(ENB , 110);       //Left motor
  digitalWrite(in3 , HIGH);
  digitalWrite(in4 , LOW);
  delay(10);

  // analogWrite(ENA , curr_RightSpeed);
  // analogWrite(ENB , curr_LeftSpeed);

  while( counter < 60){
      if(digitalRead(encoder) == 1){
          state = 1;
      }
      else{
          state = 0;
      }

      counter += state ^ last_state;
      last_state = state;

      // Serial.print("counter : ");
      // Serial.println(counter);

        ReadGyro();
        error = target_angle - AngleZ;

        change = kp * error + kd * (error - last_error);


        change_RightSpeed(change);
        change_LeftSpeed(-change);
        last_error = error;
            if(wallFront()){
              break;
            }
            digitalWrite(13 , LOW);
      }

      counter=0;
      // stop();
}
void stop(){
  analogWrite(ENA , 0);
  analogWrite(ENB , 0);

}
void turnRight(){
	// start motors
  analogWrite(ENA , 100);       // Right motor
  digitalWrite(in1 , LOW);
  digitalWrite(in2 , HIGH);

  analogWrite(ENB , 100);       //Left motor
  digitalWrite(in3 , HIGH);
  digitalWrite(in4 , LOW);

  delay(10);

  target_angle -=88.0;

  while(abs(target_angle - AngleZ)  > 16){
    analogWrite(ENA , turn_RightSpeed);       // Right motor
    analogWrite(ENB , turn_LeftSpeed);       //Left motor
      ReadGyro();

  }
  stop();
}

void turnLeft(){
	// start motors
  analogWrite(ENA , 100);       // Right motor
  digitalWrite(in1 , HIGH);
  digitalWrite(in2 , LOW);

  analogWrite(ENB , 100);       // Left motor
  digitalWrite(in3 , LOW);
  digitalWrite(in4 , HIGH);

  delay(10);

  target_angle +=88.0;

    while(abs(target_angle - AngleZ)  > 16){
    analogWrite(ENA , turn_RightSpeed);       // Right motor
    analogWrite(ENB , turn_LeftSpeed);       //Left motor
      ReadGyro();
  }

  stop();
}

void change_RightSpeed(int change){
  curr_RightSpeed += change;
  curr_RightSpeed = max(curr_RightSpeed , 47 );
  curr_RightSpeed = min(curr_RightSpeed , max_RightSpeed );
  analogWrite(ENA , curr_RightSpeed);
}

void change_LeftSpeed(int change){
  curr_LeftSpeed += change;
  curr_LeftSpeed = max(curr_LeftSpeed , 52 );
  curr_LeftSpeed = min(curr_LeftSpeed , max_LeftSpeed );
  analogWrite(ENB , curr_LeftSpeed);
}