MAX009

MeArm mounted on smart robot car controlled by 2 Arduino UNOs

Sep 5, 2016

•

45051 views

•

40 respects

Components and supplies

Arduino UNO

Apps and platforms

MAX009 Android app

Project description

Code

MAX009 arduino code for MeArm

arduino

Code to be uploaded in the arduino UNO mounted on the MeArm

1#include <Servo.h>
2
3Servo myservos[4];
4const int servosPin[]={6,9,10,11};
5const  int initialPos[]={80,90,90,0};
6const int button1Pin = 12;
7const int button2Pin  = 8;
8const int ledPin = 2;
9const int maxLineBuffer=80;
10
11String lineBuffer;
12
13int  button1PreviousState=HIGH;
14int button2PreviousState=HIGH;
15
16// action state  :
17// 0 : stop
18// 1 : turn left
19// 2 : turn right
20// 3 : go forward
21//  4 : go backward
22// 5 : go up
23// 6 : go down
24// 7 : open
25// 8 : close
26int  state=0;  
27
28void setup() {
29  pinMode(button1Pin, INPUT);
30  pinMode(button2Pin,  INPUT);
31  pinMode(ledPin, OUTPUT);
32  digitalWrite(ledPin, HIGH);
33  digitalWrite(button1Pin,  HIGH);
34  digitalWrite(button2Pin, HIGH);
35  Serial.begin(9600);
36  for (int  i=0;i<4;i++) myservos[i].attach(servosPin[i]);  
37  setInitialPos();
38}
39
40void  loop() {
41  // Listen to serial port
42  while (Serial.available() > 0) {
43
44    char c = Serial.read();
45    if (c=='\n' || lineBuffer.length()==maxLineBuffer)  {
46        lineBuffer.trim();
47        int pos=lineBuffer.indexOf(' ');
48        if (pos==-1) {  // command with no arg
49          if (lineBuffer.compareTo("tl")==0)  state=1;       // Turn Left
50          else if (lineBuffer.compareTo("tr")==0)  state=2;  // Turn Right
51          else if (lineBuffer.compareTo("fw")==0) state=3;  // move Forward
52          else if (lineBuffer.compareTo("bw")==0) state=4;  // move Backward
53          else if (lineBuffer.compareTo("up")==0) state=5;  // move Up
54          else if (lineBuffer.compareTo("do")==0) state=6;  //  move Down
55          else if (lineBuffer.compareTo("op")==0) state=7;  // OPen  claw
56          else if (lineBuffer.compareTo("cl")==0) state=8;  // CLose claw
57          else if (lineBuffer.compareTo("st")==0) state=0;  // STop
58          else  if (lineBuffer.compareTo("ip")==0) setInitialPos();  // reset to Initial Position
59          else if (lineBuffer.compareTo("rp")==0) runProgram();     // Run Program
60          else if (lineBuffer.compareTo("he")==0) printHelp();      // HElp
61        } else {  // command with one arg
62          String cmd=lineBuffer.substring(0,pos);
63          int arg=lineBuffer.substring(pos+1).toInt();
64          if (cmd.compareTo("s1")==0)  {setPos(0,arg);printPos();}       // set Servo 1 position
65          else if (cmd.compareTo("s2")==0)  {setPos(1,arg);printPos();}  // set Servo 2 position
66          else if (cmd.compareTo("s3")==0)  {setPos(2,arg);printPos();}  // set Servo 3 position
67          else if (cmd.compareTo("s4")==0)  {setPos(3,arg);printPos();}  // set Servo 4 position
68          else if (cmd.compareTo("li")==0)  setLight(arg);               // set LIght [0: OFF, 1: ON]
69        }
70        lineBuffer="";
71    }
72    else lineBuffer+=c;
73  }
74  // Check joysticks
75  if (analogRead(A0)<250)  {myservos[1].write(myservos[1].read()-1);printPos();}
76  if (analogRead(A0)>750)  {myservos[1].write(myservos[1].read()+1);printPos();}
77  if (analogRead(A1)<250)  {myservos[0].write(myservos[0].read()+1);printPos();}
78  if (analogRead(A1)>750)  {myservos[0].write(myservos[0].read()-1);printPos();}
79  if (analogRead(A2)<250)  {myservos[2].write(myservos[2].read()-1);printPos();}
80  if (analogRead(A2)>750)  {myservos[2].write(myservos[2].read()+1);printPos();}
81  if (analogRead(A3)<250)  {myservos[3].write(myservos[3].read()-1);printPos();}
82  if (analogRead(A3)>750)  {myservos[3].write(myservos[3].read()+1);printPos();}
83  // Check buttons
84  if (digitalRead(button1Pin)==LOW && button1PreviousState==HIGH) {
85    button1PreviousState=LOW;
86    setInitialPos();
87  }
88  if (digitalRead(button1Pin)==HIGH) button1PreviousState=HIGH;
89  if (digitalRead(button2Pin)==LOW && button2PreviousState==HIGH) {
90    button2PreviousState=LOW;
91    runProgram();
92  }
93  if (digitalRead(button2Pin)==HIGH) button2PreviousState=HIGH;
94  // Check state
95  switch(state) {
96    case 1:
97      myservos[0].write(myservos[0].read()+1);
98      printPos();
99      break;
100    case 2:
101      myservos[0].write(myservos[0].read()-1);
102      printPos();
103      break;      
104    case 3:
105      myservos[1].write(myservos[1].read()+1);
106      printPos();
107      break;
108    case 4:
109      myservos[1].write(myservos[1].read()-1);
110      printPos();
111      break;      
112    case 5:
113      myservos[2].write(myservos[2].read()+1);
114      printPos();
115      break;
116    case 6:
117      myservos[2].write(myservos[2].read()-1);
118      printPos();
119      break;      
120    case 7:
121      myservos[3].write(myservos[3].read()-1);
122      printPos();
123      break;
124    case 8:
125      myservos[3].write(myservos[3].read()+1);
126      printPos();
127      break;      
128  }
129  // Wait
130  delay(25);
131}
132
133void  setInitialPos() {
134  for (int i=0;i<4;i++) setPos(i,initialPos[i]);
135  printPos();
136}
137
138void  setPos(int servo_idx,int targetPos) {
139  int currentPos=myservos[servo_idx].read();
140  if (targetPos>currentPos) {
141    for (int i=currentPos;i<=targetPos;i++) {
142      myservos[servo_idx].write(i);
143      delay(25);
144    }
145  }
146  if  (targetPos<currentPos) {
147    for (int i=currentPos;i>=targetPos;i--) {
148      myservos[servo_idx].write(i);
149      delay(25);
150    }
151  }
152}
153
154void setLight(int x) {
155  digitalWrite(ledPin,!x);
156}
157
158void  runProgram() {
159  setPos(0,135);
160  setPos(0,35);
161  setPos(0,80);
162  setPos(1,120);
163  setPos(1,70);
164  setPos(1,90);
165  setPos(2,160);
166  setPos(2,70);
167  setPos(2,90);
168  setPos(3,13);
169  delay(500);
170  setPos(3,31);
171  delay(500);
172  setPos(3,13);
173  delay(500);
174  setPos(3,0);
175  printPos();
176}
177
178void printPos() {
179  for (int i=0;i<4;i++) {
180      Serial.print(myservos[i].read());
181      Serial.print("  ");
182    }
183  Serial.println();  
184}
185
186void printHelp() {
187  Serial.println("tl  : turn left");
188  Serial.println("tr : turn right"); 
189  Serial.println("fw  : move forward"); 
190  Serial.println("bw : move backward"); 
191  Serial.println("up  : move up"); 
192  Serial.println("do : move down"); 
193  Serial.println("op  : open claw"); 
194  Serial.println("cl : close claw"); 
195  Serial.println("st  : stop move");
196  Serial.println("ip : reset to initial position");
197  Serial.println("rp  : run program");
198  Serial.println("he : print help"); 
199  Serial.println("s1  [x] : move servo 1 to position x");
200  Serial.println("s2 [x] : move servo 2  to position x");
201  Serial.println("s3 [x] : move servo 3 to position x");
202  Serial.println("s4 [x] : move servo 4 to position x");
203  Serial.println("li  [x] : set light [0: OFF, 1: ON]"); 
204}
205
206

#include <Servo.h>

Servo myservos[4];
const int servosPin[]={6,9,10,11};
const  int initialPos[]={80,90,90,0};
const int button1Pin = 12;
const int button2Pin  = 8;
const int ledPin = 2;
const int maxLineBuffer=80;

String lineBuffer;

int  button1PreviousState=HIGH;
int button2PreviousState=HIGH;

// action state  :
// 0 : stop
// 1 : turn left
// 2 : turn right
// 3 : go forward
//  4 : go backward
// 5 : go up
// 6 : go down
// 7 : open
// 8 : close
int  state=0;  

void setup() {
  pinMode(button1Pin, INPUT);
  pinMode(button2Pin,  INPUT);
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, HIGH);
  digitalWrite(button1Pin,  HIGH);
  digitalWrite(button2Pin, HIGH);
  Serial.begin(9600);
  for (int  i=0;i<4;i++) myservos[i].attach(servosPin[i]);  
  setInitialPos();
}

void  loop() {
  // Listen to serial port
  while (Serial.available() > 0) {

    char c = Serial.read();
    if (c=='\n' || lineBuffer.length()==maxLineBuffer)  {
        lineBuffer.trim();
        int pos=lineBuffer.indexOf(' ');
        if (pos==-1) {  // command with no arg
          if (lineBuffer.compareTo("tl")==0)  state=1;       // Turn Left
          else if (lineBuffer.compareTo("tr")==0)  state=2;  // Turn Right
          else if (lineBuffer.compareTo("fw")==0) state=3;  // move Forward
          else if (lineBuffer.compareTo("bw")==0) state=4;  // move Backward
          else if (lineBuffer.compareTo("up")==0) state=5;  // move Up
          else if (lineBuffer.compareTo("do")==0) state=6;  //  move Down
          else if (lineBuffer.compareTo("op")==0) state=7;  // OPen  claw
          else if (lineBuffer.compareTo("cl")==0) state=8;  // CLose claw
          else if (lineBuffer.compareTo("st")==0) state=0;  // STop
          else  if (lineBuffer.compareTo("ip")==0) setInitialPos();  // reset to Initial Position
          else if (lineBuffer.compareTo("rp")==0) runProgram();     // Run Program
          else if (lineBuffer.compareTo("he")==0) printHelp();      // HElp
        } else {  // command with one arg
          String cmd=lineBuffer.substring(0,pos);
          int arg=lineBuffer.substring(pos+1).toInt();
          if (cmd.compareTo("s1")==0)  {setPos(0,arg);printPos();}       // set Servo 1 position
          else if (cmd.compareTo("s2")==0)  {setPos(1,arg);printPos();}  // set Servo 2 position
          else if (cmd.compareTo("s3")==0)  {setPos(2,arg);printPos();}  // set Servo 3 position
          else if (cmd.compareTo("s4")==0)  {setPos(3,arg);printPos();}  // set Servo 4 position
          else if (cmd.compareTo("li")==0)  setLight(arg);               // set LIght [0: OFF, 1: ON]
        }
        lineBuffer="";
    }
    else lineBuffer+=c;
  }
  // Check joysticks
  if (analogRead(A0)<250)  {myservos[1].write(myservos[1].read()-1);printPos();}
  if (analogRead(A0)>750)  {myservos[1].write(myservos[1].read()+1);printPos();}
  if (analogRead(A1)<250)  {myservos[0].write(myservos[0].read()+1);printPos();}
  if (analogRead(A1)>750)  {myservos[0].write(myservos[0].read()-1);printPos();}
  if (analogRead(A2)<250)  {myservos[2].write(myservos[2].read()-1);printPos();}
  if (analogRead(A2)>750)  {myservos[2].write(myservos[2].read()+1);printPos();}
  if (analogRead(A3)<250)  {myservos[3].write(myservos[3].read()-1);printPos();}
  if (analogRead(A3)>750)  {myservos[3].write(myservos[3].read()+1);printPos();}
  // Check buttons
  if (digitalRead(button1Pin)==LOW && button1PreviousState==HIGH) {
    button1PreviousState=LOW;
    setInitialPos();
  }
  if (digitalRead(button1Pin)==HIGH) button1PreviousState=HIGH;
  if (digitalRead(button2Pin)==LOW && button2PreviousState==HIGH) {
    button2PreviousState=LOW;
    runProgram();
  }
  if (digitalRead(button2Pin)==HIGH) button2PreviousState=HIGH;
  // Check state
  switch(state) {
    case 1:
      myservos[0].write(myservos[0].read()+1);
      printPos();
      break;
    case 2:
      myservos[0].write(myservos[0].read()-1);
      printPos();
      break;      
    case 3:
      myservos[1].write(myservos[1].read()+1);
      printPos();
      break;
    case 4:
      myservos[1].write(myservos[1].read()-1);
      printPos();
      break;      
    case 5:
      myservos[2].write(myservos[2].read()+1);
      printPos();
      break;
    case 6:
      myservos[2].write(myservos[2].read()-1);
      printPos();
      break;      
    case 7:
      myservos[3].write(myservos[3].read()-1);
      printPos();
      break;
    case 8:
      myservos[3].write(myservos[3].read()+1);
      printPos();
      break;      
  }
  // Wait
  delay(25);
}

void  setInitialPos() {
  for (int i=0;i<4;i++) setPos(i,initialPos[i]);
  printPos();
}

void  setPos(int servo_idx,int targetPos) {
  int currentPos=myservos[servo_idx].read();
  if (targetPos>currentPos) {
    for (int i=currentPos;i<=targetPos;i++) {
      myservos[servo_idx].write(i);
      delay(25);
    }
  }
  if  (targetPos<currentPos) {
    for (int i=currentPos;i>=targetPos;i--) {
      myservos[servo_idx].write(i);
      delay(25);
    }
  }
}

void setLight(int x) {
  digitalWrite(ledPin,!x);
}

void  runProgram() {
  setPos(0,135);
  setPos(0,35);
  setPos(0,80);
  setPos(1,120);
  setPos(1,70);
  setPos(1,90);
  setPos(2,160);
  setPos(2,70);
  setPos(2,90);
  setPos(3,13);
  delay(500);
  setPos(3,31);
  delay(500);
  setPos(3,13);
  delay(500);
  setPos(3,0);
  printPos();
}

void printPos() {
  for (int i=0;i<4;i++) {
      Serial.print(myservos[i].read());
      Serial.print("  ");
    }
  Serial.println();  
}

void printHelp() {
  Serial.println("tl  : turn left");
  Serial.println("tr : turn right"); 
  Serial.println("fw  : move forward"); 
  Serial.println("bw : move backward"); 
  Serial.println("up  : move up"); 
  Serial.println("do : move down"); 
  Serial.println("op  : open claw"); 
  Serial.println("cl : close claw"); 
  Serial.println("st  : stop move");
  Serial.println("ip : reset to initial position");
  Serial.println("rp  : run program");
  Serial.println("he : print help"); 
  Serial.println("s1  [x] : move servo 1 to position x");
  Serial.println("s2 [x] : move servo 2  to position x");
  Serial.println("s3 [x] : move servo 3 to position x");
  Serial.println("s4 [x] : move servo 4 to position x");
  Serial.println("li  [x] : set light [0: OFF, 1: ON]"); 
}

MAX009 arduino code for MeArm

arduino

Code to be uploaded in the arduino UNO mounted on the MeArm

1#include <Servo.h>
2
3Servo myservos[4];
4const int servosPin[]={6,9,10,11};
5const
6  int initialPos[]={80,90,90,0};
7const int button1Pin = 12;
8const int button2Pin
9  = 8;
10const int ledPin = 2;
11const int maxLineBuffer=80;
12
13String lineBuffer;
14
15int
16  button1PreviousState=HIGH;
17int button2PreviousState=HIGH;
18
19// action state
20  :
21// 0 : stop
22// 1 : turn left
23// 2 : turn right
24// 3 : go forward
25//
26  4 : go backward
27// 5 : go up
28// 6 : go down
29// 7 : open
30// 8 : close
31int
32  state=0;  
33
34void setup() {
35  pinMode(button1Pin, INPUT);
36  pinMode(button2Pin,
37  INPUT);
38  pinMode(ledPin, OUTPUT);
39  digitalWrite(ledPin, HIGH);
40  digitalWrite(button1Pin,
41  HIGH);
42  digitalWrite(button2Pin, HIGH);
43  Serial.begin(9600);
44  for (int
45  i=0;i<4;i++) myservos[i].attach(servosPin[i]);  
46  setInitialPos();
47}
48
49void
50  loop() {
51  // Listen to serial port
52  while (Serial.available() > 0) {
53
54
55    char c = Serial.read();
56    if (c=='\
57' || lineBuffer.length()==maxLineBuffer)
58  {
59        lineBuffer.trim();
60        int pos=lineBuffer.indexOf(' ');
61
62        if (pos==-1) {  // command with no arg
63          if (lineBuffer.compareTo("tl")==0)
64  state=1;       // Turn Left
65          else if (lineBuffer.compareTo("tr")==0)
66  state=2;  // Turn Right
67          else if (lineBuffer.compareTo("fw")==0) state=3;
68  // move Forward
69          else if (lineBuffer.compareTo("bw")==0) state=4;
70  // move Backward
71          else if (lineBuffer.compareTo("up")==0) state=5;
72  // move Up
73          else if (lineBuffer.compareTo("do")==0) state=6;  //
74  move Down
75          else if (lineBuffer.compareTo("op")==0) state=7;  // OPen
76  claw
77          else if (lineBuffer.compareTo("cl")==0) state=8;  // CLose claw
78
79          else if (lineBuffer.compareTo("st")==0) state=0;  // STop
80          else
81  if (lineBuffer.compareTo("ip")==0) setInitialPos();  // reset to Initial Position
82
83          else if (lineBuffer.compareTo("rp")==0) runProgram();     // Run Program
84
85          else if (lineBuffer.compareTo("he")==0) printHelp();      // HElp
86
87        } else {  // command with one arg
88          String cmd=lineBuffer.substring(0,pos);
89
90          int arg=lineBuffer.substring(pos+1).toInt();
91          if (cmd.compareTo("s1")==0)
92  {setPos(0,arg);printPos();}       // set Servo 1 position
93          else if (cmd.compareTo("s2")==0)
94  {setPos(1,arg);printPos();}  // set Servo 2 position
95          else if (cmd.compareTo("s3")==0)
96  {setPos(2,arg);printPos();}  // set Servo 3 position
97          else if (cmd.compareTo("s4")==0)
98  {setPos(3,arg);printPos();}  // set Servo 4 position
99          else if (cmd.compareTo("li")==0)
100  setLight(arg);               // set LIght [0: OFF, 1: ON]
101        }
102        lineBuffer="";
103
104    }
105    else lineBuffer+=c;
106  }
107  // Check joysticks
108  if (analogRead(A0)<250)
109  {myservos[1].write(myservos[1].read()-1);printPos();}
110  if (analogRead(A0)>750)
111  {myservos[1].write(myservos[1].read()+1);printPos();}
112  if (analogRead(A1)<250)
113  {myservos[0].write(myservos[0].read()+1);printPos();}
114  if (analogRead(A1)>750)
115  {myservos[0].write(myservos[0].read()-1);printPos();}
116  if (analogRead(A2)<250)
117  {myservos[2].write(myservos[2].read()-1);printPos();}
118  if (analogRead(A2)>750)
119  {myservos[2].write(myservos[2].read()+1);printPos();}
120  if (analogRead(A3)<250)
121  {myservos[3].write(myservos[3].read()-1);printPos();}
122  if (analogRead(A3)>750)
123  {myservos[3].write(myservos[3].read()+1);printPos();}
124  // Check buttons
125
126  if (digitalRead(button1Pin)==LOW && button1PreviousState==HIGH) {
127    button1PreviousState=LOW;
128
129    setInitialPos();
130  }
131  if (digitalRead(button1Pin)==HIGH) button1PreviousState=HIGH;
132
133  if (digitalRead(button2Pin)==LOW && button2PreviousState==HIGH) {
134    button2PreviousState=LOW;
135
136    runProgram();
137  }
138  if (digitalRead(button2Pin)==HIGH) button2PreviousState=HIGH;
139
140  // Check state
141  switch(state) {
142    case 1:
143      myservos[0].write(myservos[0].read()+1);
144
145      printPos();
146      break;
147    case 2:
148      myservos[0].write(myservos[0].read()-1);
149
150      printPos();
151      break;      
152    case 3:
153      myservos[1].write(myservos[1].read()+1);
154
155      printPos();
156      break;
157    case 4:
158      myservos[1].write(myservos[1].read()-1);
159
160      printPos();
161      break;      
162    case 5:
163      myservos[2].write(myservos[2].read()+1);
164
165      printPos();
166      break;
167    case 6:
168      myservos[2].write(myservos[2].read()-1);
169
170      printPos();
171      break;      
172    case 7:
173      myservos[3].write(myservos[3].read()-1);
174
175      printPos();
176      break;
177    case 8:
178      myservos[3].write(myservos[3].read()+1);
179
180      printPos();
181      break;      
182  }
183  // Wait
184  delay(25);
185}
186
187void
188  setInitialPos() {
189  for (int i=0;i<4;i++) setPos(i,initialPos[i]);
190  printPos();
191}
192
193void
194  setPos(int servo_idx,int targetPos) {
195  int currentPos=myservos[servo_idx].read();
196
197  if (targetPos>currentPos) {
198    for (int i=currentPos;i<=targetPos;i++) {
199
200      myservos[servo_idx].write(i);
201      delay(25);
202    }
203  }
204  if
205  (targetPos<currentPos) {
206    for (int i=currentPos;i>=targetPos;i--) {
207      myservos[servo_idx].write(i);
208
209      delay(25);
210    }
211  }
212}
213
214void setLight(int x) {
215  digitalWrite(ledPin,!x);
216}
217
218void
219  runProgram() {
220  setPos(0,135);
221  setPos(0,35);
222  setPos(0,80);
223  setPos(1,120);
224
225  setPos(1,70);
226  setPos(1,90);
227  setPos(2,160);
228  setPos(2,70);
229  setPos(2,90);
230
231  setPos(3,13);
232  delay(500);
233  setPos(3,31);
234  delay(500);
235  setPos(3,13);
236
237  delay(500);
238  setPos(3,0);
239  printPos();
240}
241
242void printPos() {
243
244  for (int i=0;i<4;i++) {
245      Serial.print(myservos[i].read());
246      Serial.print("
247  ");
248    }
249  Serial.println();  
250}
251
252void printHelp() {
253  Serial.println("tl
254  : turn left");
255  Serial.println("tr : turn right"); 
256  Serial.println("fw
257  : move forward"); 
258  Serial.println("bw : move backward"); 
259  Serial.println("up
260  : move up"); 
261  Serial.println("do : move down"); 
262  Serial.println("op
263  : open claw"); 
264  Serial.println("cl : close claw"); 
265  Serial.println("st
266  : stop move");
267  Serial.println("ip : reset to initial position");
268  Serial.println("rp
269  : run program");
270  Serial.println("he : print help"); 
271  Serial.println("s1
272  [x] : move servo 1 to position x");
273  Serial.println("s2 [x] : move servo 2
274  to position x");
275  Serial.println("s3 [x] : move servo 3 to position x");
276
277  Serial.println("s4 [x] : move servo 4 to position x");
278  Serial.println("li
279  [x] : set light [0: OFF, 1: ON]"); 
280}
281
282

#include <Servo.h>

Servo myservos[4];
const int servosPin[]={6,9,10,11};
const
  int initialPos[]={80,90,90,0};
const int button1Pin = 12;
const int button2Pin
  = 8;
const int ledPin = 2;
const int maxLineBuffer=80;

String lineBuffer;

int
  button1PreviousState=HIGH;
int button2PreviousState=HIGH;

// action state
  :
// 0 : stop
// 1 : turn left
// 2 : turn right
// 3 : go forward
//
  4 : go backward
// 5 : go up
// 6 : go down
// 7 : open
// 8 : close
int
  state=0;  

void setup() {
  pinMode(button1Pin, INPUT);
  pinMode(button2Pin,
  INPUT);
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, HIGH);
  digitalWrite(button1Pin,
  HIGH);
  digitalWrite(button2Pin, HIGH);
  Serial.begin(9600);
  for (int
  i=0;i<4;i++) myservos[i].attach(servosPin[i]);  
  setInitialPos();
}

void
  loop() {
  // Listen to serial port
  while (Serial.available() > 0) {


    char c = Serial.read();
    if (c=='\
' || lineBuffer.length()==maxLineBuffer)
  {
        lineBuffer.trim();
        int pos=lineBuffer.indexOf(' ');

        if (pos==-1) {  // command with no arg
          if (lineBuffer.compareTo("tl")==0)
  state=1;       // Turn Left
          else if (lineBuffer.compareTo("tr")==0)
  state=2;  // Turn Right
          else if (lineBuffer.compareTo("fw")==0) state=3;
  // move Forward
          else if (lineBuffer.compareTo("bw")==0) state=4;
  // move Backward
          else if (lineBuffer.compareTo("up")==0) state=5;
  // move Up
          else if (lineBuffer.compareTo("do")==0) state=6;  //
  move Down
          else if (lineBuffer.compareTo("op")==0) state=7;  // OPen
  claw
          else if (lineBuffer.compareTo("cl")==0) state=8;  // CLose claw

          else if (lineBuffer.compareTo("st")==0) state=0;  // STop
          else
  if (lineBuffer.compareTo("ip")==0) setInitialPos();  // reset to Initial Position

          else if (lineBuffer.compareTo("rp")==0) runProgram();     // Run Program

          else if (lineBuffer.compareTo("he")==0) printHelp();      // HElp

        } else {  // command with one arg
          String cmd=lineBuffer.substring(0,pos);

          int arg=lineBuffer.substring(pos+1).toInt();
          if (cmd.compareTo("s1")==0)
  {setPos(0,arg);printPos();}       // set Servo 1 position
          else if (cmd.compareTo("s2")==0)
  {setPos(1,arg);printPos();}  // set Servo 2 position
          else if (cmd.compareTo("s3")==0)
  {setPos(2,arg);printPos();}  // set Servo 3 position
          else if (cmd.compareTo("s4")==0)
  {setPos(3,arg);printPos();}  // set Servo 4 position
          else if (cmd.compareTo("li")==0)
  setLight(arg);               // set LIght [0: OFF, 1: ON]
        }
        lineBuffer="";

    }
    else lineBuffer+=c;
  }
  // Check joysticks
  if (analogRead(A0)<250)
  {myservos[1].write(myservos[1].read()-1);printPos();}
  if (analogRead(A0)>750)
  {myservos[1].write(myservos[1].read()+1);printPos();}
  if (analogRead(A1)<250)
  {myservos[0].write(myservos[0].read()+1);printPos();}
  if (analogRead(A1)>750)
  {myservos[0].write(myservos[0].read()-1);printPos();}
  if (analogRead(A2)<250)
  {myservos[2].write(myservos[2].read()-1);printPos();}
  if (analogRead(A2)>750)
  {myservos[2].write(myservos[2].read()+1);printPos();}
  if (analogRead(A3)<250)
  {myservos[3].write(myservos[3].read()-1);printPos();}
  if (analogRead(A3)>750)
  {myservos[3].write(myservos[3].read()+1);printPos();}
  // Check buttons

  if (digitalRead(button1Pin)==LOW && button1PreviousState==HIGH) {
    button1PreviousState=LOW;

    setInitialPos();
  }
  if (digitalRead(button1Pin)==HIGH) button1PreviousState=HIGH;

  if (digitalRead(button2Pin)==LOW && button2PreviousState==HIGH) {
    button2PreviousState=LOW;

    runProgram();
  }
  if (digitalRead(button2Pin)==HIGH) button2PreviousState=HIGH;

  // Check state
  switch(state) {
    case 1:
      myservos[0].write(myservos[0].read()+1);

      printPos();
      break;
    case 2:
      myservos[0].write(myservos[0].read()-1);

      printPos();
      break;      
    case 3:
      myservos[1].write(myservos[1].read()+1);

      printPos();
      break;
    case 4:
      myservos[1].write(myservos[1].read()-1);

      printPos();
      break;      
    case 5:
      myservos[2].write(myservos[2].read()+1);

      printPos();
      break;
    case 6:
      myservos[2].write(myservos[2].read()-1);

      printPos();
      break;      
    case 7:
      myservos[3].write(myservos[3].read()-1);

      printPos();
      break;
    case 8:
      myservos[3].write(myservos[3].read()+1);

      printPos();
      break;      
  }
  // Wait
  delay(25);
}

void
  setInitialPos() {
  for (int i=0;i<4;i++) setPos(i,initialPos[i]);
  printPos();
}

void
  setPos(int servo_idx,int targetPos) {
  int currentPos=myservos[servo_idx].read();

  if (targetPos>currentPos) {
    for (int i=currentPos;i<=targetPos;i++) {

      myservos[servo_idx].write(i);
      delay(25);
    }
  }
  if
  (targetPos<currentPos) {
    for (int i=currentPos;i>=targetPos;i--) {
      myservos[servo_idx].write(i);

      delay(25);
    }
  }
}

void setLight(int x) {
  digitalWrite(ledPin,!x);
}

void
  runProgram() {
  setPos(0,135);
  setPos(0,35);
  setPos(0,80);
  setPos(1,120);

  setPos(1,70);
  setPos(1,90);
  setPos(2,160);
  setPos(2,70);
  setPos(2,90);

  setPos(3,13);
  delay(500);
  setPos(3,31);
  delay(500);
  setPos(3,13);

  delay(500);
  setPos(3,0);
  printPos();
}

void printPos() {

  for (int i=0;i<4;i++) {
      Serial.print(myservos[i].read());
      Serial.print("
  ");
    }
  Serial.println();  
}

void printHelp() {
  Serial.println("tl
  : turn left");
  Serial.println("tr : turn right"); 
  Serial.println("fw
  : move forward"); 
  Serial.println("bw : move backward"); 
  Serial.println("up
  : move up"); 
  Serial.println("do : move down"); 
  Serial.println("op
  : open claw"); 
  Serial.println("cl : close claw"); 
  Serial.println("st
  : stop move");
  Serial.println("ip : reset to initial position");
  Serial.println("rp
  : run program");
  Serial.println("he : print help"); 
  Serial.println("s1
  [x] : move servo 1 to position x");
  Serial.println("s2 [x] : move servo 2
  to position x");
  Serial.println("s3 [x] : move servo 3 to position x");

  Serial.println("s4 [x] : move servo 4 to position x");
  Serial.println("li
  [x] : set light [0: OFF, 1: ON]"); 
}

MAX009 arduino code for smart robot car

arduino

Code to be uploaded in the arduino UNO mounted on the smart robot car

1#include <MsTimer2.h>
2#include <EEPROM.h>
3#include <SoftwareSerial.h>
4
5const  int IntPin0=2;
6const int IntPin1=3;
7const int IntDebounce=50;
8const int  MotorRDirectionPin =  4; 
9const int MotorRSpeedPin =  5; 
10const int MotorLDirectionPin  =  7; 
11const int MotorLSpeedPin =  6;
12const int ProxSensor1Pin = 8;
13const  int ProxSensor2Pin = 9;
14const int EchoTriggerPin = 10;
15const int EchoPin =  11;
16SoftwareSerial softSerial(100,12); // RX, TX
17const int ButtonPin = 13;
18const  int HeadLightsPin = A0;
19const int HornPin = A1;
20const int LeftLightPin = A2;
21const  int leftRearLightPin = A3;
22const int rightRearLightPin = A4;
23const int RightLightPin  = A5;
24const int maxLineBuffer=80;
25double Kc=0.1;
26unsigned int speedTarget=150;
27const  int rotPwm=130;
28unsigned int dist=0;
29
30char c;
31String lineBuffer;
32String  pgrmString="";
33int inputMode=0; // 0 = normal/interactive, 1 = record program
34double  pwmInit=130;
35double pwmL=pwmInit;
36double lasterrorL=0;
37double pwmR=pwmInit;
38double  lasterrorR=0;
39int stepCntTarget=0;
40boolean useProxSensor=0;
41
42volatile  int PIDstate=0;  // 0 = stop, 1 = forward, 2 = backward
43volatile unsigned long  prevIntTimeL;
44volatile unsigned long intPeriodL;
45volatile unsigned long prevIntTimeR;
46volatile  unsigned long intPeriodR;
47volatile unsigned int stepCntL=0;
48volatile unsigned  int stepCntR=0;
49unsigned long PIDi=0;
50
51boolean LeftLightState=false;
52boolean  RightLightState=false;
53int SideLightCnt=0;
54
55void setup() {
56  pinMode(MotorRDirectionPin,  OUTPUT);
57  pinMode(MotorRSpeedPin, OUTPUT);
58  pinMode(MotorLDirectionPin,  OUTPUT);
59  pinMode(MotorLSpeedPin, OUTPUT);
60  digitalWrite(MotorRDirectionPin,LOW);
61  digitalWrite(MotorRSpeedPin,LOW);
62  digitalWrite(MotorLDirectionPin,LOW);
63  digitalWrite(MotorLSpeedPin,LOW);
64  pinMode(ButtonPin, INPUT);
65  digitalWrite(ButtonPin,  HIGH);
66  pinMode(IntPin0,INPUT);
67  digitalWrite(IntPin0,HIGH);
68  pinMode(IntPin1,INPUT);
69  digitalWrite(IntPin1,HIGH);
70  attachInterrupt(0, ISR0, RISING);
71  attachInterrupt(1,  ISR1, RISING);
72  pinMode(ProxSensor1Pin,INPUT);
73  digitalWrite(ProxSensor1Pin,HIGH);  
74  pinMode(ProxSensor2Pin,INPUT);
75  digitalWrite(ProxSensor2Pin,HIGH);  
76  pinMode(EchoPin,INPUT);
77  pinMode(EchoTriggerPin,OUTPUT);
78  digitalWrite(EchoTriggerPin,LOW);
79  pinMode(HeadLightsPin,OUTPUT);
80  digitalWrite(HeadLightsPin,LOW);
81  pinMode(HornPin,OUTPUT);
82  digitalWrite(HornPin,LOW);
83  pinMode(LeftLightPin,OUTPUT);
84  digitalWrite(LeftLightPin,LOW);
85  pinMode(RightLightPin,OUTPUT);
86  digitalWrite(RightLightPin,LOW);
87  pinMode(leftRearLightPin,OUTPUT);
88  digitalWrite(leftRearLightPin,LOW);
89  pinMode(rightRearLightPin,OUTPUT);
90  digitalWrite(rightRearLightPin,LOW);
91    
92  if (digitalRead(ProxSensor1Pin)==LOW  || digitalRead(ProxSensor2Pin)==LOW) {
93    useProxSensor=false;
94    horn(1);
95    delay(100);
96    horn(0);
97  } else {
98    useProxSensor=true;
99    horn(1);
100    delay(100);
101    horn(0);
102    delay(100);
103    horn(1);
104    delay(100);
105    horn(0);
106  }
107
108  Serial.begin(115200);
109  // Load program from eeprom
110  int i=0;
111  c=EEPROM.read(i);
112  while(c!=0) {
113    pgrmString+=c;
114    i++;
115    c=EEPROM.read(i);
116  }
117
118  softSerial.begin(9600);
119  
120  // Timer  interrupt
121  MsTimer2::set(50, timerISR); 
122  MsTimer2::start();
123}
124
125void  loop() {
126  while (Serial.available() > 0) {
127    c = Serial.read();
128    if  (c=='\n' || lineBuffer.length()==maxLineBuffer) {
129      if (inputMode==1) {
130        lineBuffer.trim();
131        if (lineBuffer.compareTo("ep")==0) {
132          // Save program to eeprom
133          for (int i=0;i<pgrmString.length();i++)  EEPROM.write(i,pgrmString.charAt(i));
134          EEPROM.write(pgrmString.length(),0);
135          Serial.println("Program recorded : ");
136          Serial.println(pgrmString);
137          inputMode=0;
138        } else {  
139          if (!lineBuffer.endsWith(";")  && !lineBuffer.endsWith("]")) lineBuffer+=';';
140          pgrmString=pgrmString+lineBuffer;
141        }
142      } else execPgrm(lineBuffer);
143      lineBuffer="";
144    }
145    else lineBuffer+=c;
146  }
147  if (digitalRead(ButtonPin)==LOW) execPgrm(pgrmString);
148}
149
150void  moveForward() {
151  PIDstate=1;
152  PIDi=0;
153  pwmL=pwmInit;
154  pwmR=pwmInit;
155  digitalWrite(MotorLDirectionPin,LOW);
156  analogWrite(MotorLSpeedPin,pwmL);    
157  digitalWrite(MotorRDirectionPin,LOW);
158  analogWrite(MotorRSpeedPin,pwmR);  
159}
160
161void  moveBackward() {
162  PIDstate=2;
163  PIDi=0;
164  digitalWrite(MotorLDirectionPin,HIGH);
165  analogWrite(MotorLSpeedPin,255-pwmL);
166  digitalWrite(MotorRDirectionPin,HIGH);
167  analogWrite(MotorRSpeedPin,255-pwmR);
168}
169
170void turnRight() { 
171  digitalWrite(MotorLDirectionPin,LOW);
172  analogWrite(MotorLSpeedPin,rotPwm);
173  digitalWrite(MotorRDirectionPin,HIGH);
174  analogWrite(MotorRSpeedPin,255-rotPwm);
175}
176
177void turnLeft() {
178  digitalWrite(MotorRDirectionPin,LOW);
179  analogWrite(MotorRSpeedPin,rotPwm);
180  digitalWrite(MotorLDirectionPin,HIGH);
181  analogWrite(MotorLSpeedPin,255-rotPwm);
182}
183
184void stopMove() {
185  PIDstate=0;
186  digitalWrite(MotorRDirectionPin,LOW);
187  digitalWrite(MotorRSpeedPin,LOW);
188  digitalWrite(MotorLDirectionPin,LOW);
189  digitalWrite(MotorLSpeedPin,LOW);
190}
191
192void  moveForward(int x) {
193  stepCntTarget=x;
194  stepCntL=0;
195  stepCntR=0;
196  PIDi=0;
197  moveForward();  
198  while(stepCntL<stepCntTarget && (!useProxSensor  || (digitalRead(ProxSensor1Pin) && digitalRead(ProxSensor2Pin)))) { 
199    digitalWrite(EchoTriggerPin,  HIGH);
200    delayMicroseconds(10);
201    digitalWrite(EchoTriggerPin, LOW); 
202    dist=pulseIn(EchoPin,HIGH,60000)/58;
203    if (dist>0&&dist<20) break;
204  }
205  stopMove();
206}
207
208void moveBackward(int x) {
209  stepCntTarget=x;
210  stepCntL=0;
211  stepCntR=0; 
212  PIDi=0;
213  moveBackward();
214  while(stepCntL<stepCntTarget);  
215  stopMove();
216}
217
218void turnLeft(int x) {
219  stepCntTarget=x;
220  stepCntL=0;
221  stepCntR=0;   
222  turnLeft();
223  while(stepCntL<stepCntTarget);  
224  stopMove();  
225}
226
227void turnRight(int x) {
228  stepCntTarget=x;
229  stepCntL=0;
230  stepCntR=0;   
231  turnRight();
232  while(stepCntL<stepCntTarget);  
233  stopMove();
234}
235
236void pause(int x) {
237  delay(x);
238}
239
240void  setkc(int x) {
241  Kc=double(x)/1000;
242}
243
244void setPwmInit(int x) {
245  pwmInit=x;
246}
247
248void setspeed(int x) {
249  speedTarget=x;
250}
251
252void  leftLight(int x) {
253    if (x==0) {
254      LeftLightState=false;
255      digitalWrite(LeftLightPin,LOW);
256    }
257    else if (x==2) {
258      LeftLightState=false;
259      digitalWrite(LeftLightPin,HIGH);
260    }
261    else {
262        digitalWrite(LeftLightPin,HIGH);
263        LeftLightState=true;
264        if (!RightLightState) SideLightCnt=0;
265    }
266}
267
268void rightLight(int  x) {
269    if (x==0) {
270      RightLightState=false;
271      digitalWrite(RightLightPin,LOW);
272    }
273    else if (x==2) {
274      RightLightState=false;
275      digitalWrite(RightLightPin,HIGH);
276    }
277    else {
278      digitalWrite(RightLightPin,HIGH);
279      RightLightState=true;
280      if (!LeftLightState) SideLightCnt=0;
281    }
282    
283}
284
285void leftRearLight(int  x) {
286  if (x==0) digitalWrite(leftRearLightPin,LOW);
287    else digitalWrite(leftRearLightPin,HIGH);
288}
289
290void  rightRearLight(int x) {
291  if (x==0) digitalWrite(rightRearLightPin,LOW);
292    else digitalWrite(rightRearLightPin,HIGH);
293}
294
295void headLight(int x)  {
296  if (x==0) digitalWrite(HeadLightsPin,LOW);
297    else digitalWrite(HeadLightsPin,HIGH);
298}
299
300void  horn(int x) {
301  if (x==0) digitalWrite(HornPin,LOW);
302    else digitalWrite(HornPin,HIGH);
303}
304
305void  help() {
306  Serial.println("fw (n) : move forward (by n steps)");
307  Serial.println("bw  (n) : move backward (by n steps)");
308  Serial.println("tl (n) : turn left (by  n steps)");
309  Serial.println("tr (n) : turn right (by n steps)");
310  Serial.println("st  : stop move");
311  Serial.println("pa [n] : pause of n milliseconds");
312  Serial.println("hl  [x] : head light [0: OFF, 1: ON]");
313  Serial.println("ho [x] : horn [0: OFF,  1: ON]");
314  Serial.println("lr [x] : left rear light [0: OFF, 1: ON]");
315  Serial.println("rr [x] : right read light [0: OFF, 1: ON]");
316  Serial.println("ll  [x] : left light [0: OFF, 1: CLIGN, 2:ON]");
317  Serial.println("rl [x] : right  light [0: OFF, 1: CLIGN, 2:ON]");
318  Serial.println("ss [n] : set speed");
319  Serial.println("sp : save program");
320  Serial.println("ep : end of program");
321  Serial.println("rp : run program");
322  Serial.println("pp : print program");
323  Serial.println("ma.... : forward command ... to MeArm");
324  Serial.println("he  : help");
325}
326
327
328void processCmd(String str) {
329  Serial.print(str);
330  Serial.print(" : ");
331
332  int pos=str.indexOf(' ');
333  boolean ok=1;
334  
335  if (str.startsWith("ma")) softSerial.println(str.substring(2)); // Forward  command to MeArm
336  else {
337    if (pos==-1) {  // command with no arg
338      if  (str.compareTo("fw")==0 || str.compareTo("8")==0) moveForward();
339      else  if (str.compareTo("bw")==0 || str.compareTo("2")==0) moveBackward();
340      else  if (str.compareTo("tl")==0 || str.compareTo("4")==0) turnLeft();
341      else  if (str.compareTo("tr")==0 || str.compareTo("6")==0) turnRight();
342      else  if (str.compareTo("st")==0 || str.compareTo("5")==0) stopMove();
343      else  if (str.compareTo("sp")==0) {pgrmString="";inputMode=1;}
344      else if (str.compareTo("rp")==0)  execPgrm(pgrmString);
345      else if (str.compareTo("pp")==0) Serial.println(pgrmString);
346      else if (str.compareTo("he")==0) help();
347      else ok=0;
348    } else  {  // command with at least 1 arg
349      String cmd=str.substring(0,pos);
350      int arg=str.substring(pos+1).toInt();
351      if (cmd.compareTo("fw")==0)  moveForward(arg);
352      else if (cmd.compareTo("bw")==0) moveBackward(arg);
353      else if (cmd.compareTo("tl")==0) turnLeft(arg);
354      else if (cmd.compareTo("tr")==0)  turnRight(arg);
355      else if (cmd.compareTo("pa")==0) pause(arg);
356      else  if (cmd.compareTo("kc")==0) setkc(arg);    
357      else if (cmd.compareTo("ss")==0)  setspeed(arg);        
358      else if (cmd.compareTo("ll")==0) leftLight(arg);        
359      else if (cmd.compareTo("rl")==0) rightLight(arg);
360      else  if (cmd.compareTo("lr")==0) leftRearLight(arg);        
361      else if (cmd.compareTo("rr")==0)  rightRearLight(arg);
362      else if (cmd.compareTo("hl")==0) headLight(arg);
363      else if (cmd.compareTo("ho")==0) horn(arg);
364      else ok=0;
365    }
366  }
367  if (ok) Serial.println("OK");
368    else Serial.println("Error");
369  if (inputMode==1) Serial.println("Recording program");
370}
371
372void execPgrm(String  str) {
373  while(str.length()>0) {
374    int endpos;
375    str.trim();
376    if  (str.startsWith("repeat")) {
377      int startpos=str.indexOf('[');
378      int  n=str.substring(6,startpos).toInt();
379      int level=1;
380      endpos=startpos;
381      while(endpos<str.length()&&level>0) {
382        endpos++;
383        if (str.charAt(endpos)=='[')  level++;
384        if (str.charAt(endpos)==']') level--;
385      }
386      for  (int i=0;i<n;i++) execPgrm(str.substring(startpos+1,endpos));
387    } else {
388      endpos=str.indexOf(';');
389      if (endpos==-1) endpos=str.length();
390      processCmd(str.substring(0,endpos));
391    }
392    if (endpos<str.length()-1)  str=str.substring(endpos+1);
393      else str="";
394  }
395}
396
397void ISR0()  {
398  unsigned long intTime=micros();
399  if ((intTime-prevIntTimeR)>IntDebounce  && digitalRead(IntPin0)==HIGH) {
400    intPeriodR=intTime-prevIntTimeR;
401    prevIntTimeR=intTime;
402    stepCntR++;
403  }
404}
405
406void ISR1() {
407  unsigned long intTime=micros();
408  if ((intTime-prevIntTimeL)>IntDebounce && digitalRead(IntPin1)==HIGH) {
409    intPeriodL=intTime-prevIntTimeL;
410    prevIntTimeL=intTime;
411    stepCntL++;    
412  }
413}
414
415void timerISR()  {
416  if (PIDstate>0) {
417    double target=speedTarget;
418    if (PIDi<8) target=target*PIDi/8;
419    double meas=3e6/double(intPeriodL);
420    if (meas>1000) meas=0;
421    double  error=target-meas;
422    pwmL=pwmL+Kc*error;
423    if (pwmL>255) pwmL=255;
424    if (pwmL<0) pwmL=0;
425    lasterrorL=error;
426    if (PIDstate==2) analogWrite(MotorLSpeedPin,255-pwmL);
427      else analogWrite(MotorLSpeedPin,pwmL);
428  
429    meas=3e6/double(intPeriodR);
430    if (meas>1000) meas=0;
431    error=target-meas;
432    pwmR=pwmR+Kc*error;
433    if (pwmR>255) pwmR=255;
434    if (pwmR<0) pwmR=0;
435    lasterrorR=error;
436    if (PIDstate==2) analogWrite(MotorRSpeedPin,255-pwmR);
437      else analogWrite(MotorRSpeedPin,pwmR);
438
439    PIDi++;    
440  }
441
442  SideLightCnt++;
443  if (LeftLightState && (SideLightCnt%10)==0)  digitalWrite(LeftLightPin,(SideLightCnt%20)==0);
444  if (RightLightState && (SideLightCnt%10)==0)  digitalWrite(RightLightPin,(SideLightCnt%20)==0);
445}
446

#include <MsTimer2.h>
#include <EEPROM.h>
#include <SoftwareSerial.h>

const  int IntPin0=2;
const int IntPin1=3;
const int IntDebounce=50;
const int  MotorRDirectionPin =  4; 
const int MotorRSpeedPin =  5; 
const int MotorLDirectionPin  =  7; 
const int MotorLSpeedPin =  6;
const int ProxSensor1Pin = 8;
const  int ProxSensor2Pin = 9;
const int EchoTriggerPin = 10;
const int EchoPin =  11;
SoftwareSerial softSerial(100,12); // RX, TX
const int ButtonPin = 13;
const  int HeadLightsPin = A0;
const int HornPin = A1;
const int LeftLightPin = A2;
const  int leftRearLightPin = A3;
const int rightRearLightPin = A4;
const int RightLightPin  = A5;
const int maxLineBuffer=80;
double Kc=0.1;
unsigned int speedTarget=150;
const  int rotPwm=130;
unsigned int dist=0;

char c;
String lineBuffer;
String  pgrmString="";
int inputMode=0; // 0 = normal/interactive, 1 = record program
double  pwmInit=130;
double pwmL=pwmInit;
double lasterrorL=0;
double pwmR=pwmInit;
double  lasterrorR=0;
int stepCntTarget=0;
boolean useProxSensor=0;

volatile  int PIDstate=0;  // 0 = stop, 1 = forward, 2 = backward
volatile unsigned long  prevIntTimeL;
volatile unsigned long intPeriodL;
volatile unsigned long prevIntTimeR;
volatile  unsigned long intPeriodR;
volatile unsigned int stepCntL=0;
volatile unsigned  int stepCntR=0;
unsigned long PIDi=0;

boolean LeftLightState=false;
boolean  RightLightState=false;
int SideLightCnt=0;

void setup() {
  pinMode(MotorRDirectionPin,  OUTPUT);
  pinMode(MotorRSpeedPin, OUTPUT);
  pinMode(MotorLDirectionPin,  OUTPUT);
  pinMode(MotorLSpeedPin, OUTPUT);
  digitalWrite(MotorRDirectionPin,LOW);
  digitalWrite(MotorRSpeedPin,LOW);
  digitalWrite(MotorLDirectionPin,LOW);
  digitalWrite(MotorLSpeedPin,LOW);
  pinMode(ButtonPin, INPUT);
  digitalWrite(ButtonPin,  HIGH);
  pinMode(IntPin0,INPUT);
  digitalWrite(IntPin0,HIGH);
  pinMode(IntPin1,INPUT);
  digitalWrite(IntPin1,HIGH);
  attachInterrupt(0, ISR0, RISING);
  attachInterrupt(1,  ISR1, RISING);
  pinMode(ProxSensor1Pin,INPUT);
  digitalWrite(ProxSensor1Pin,HIGH);  
  pinMode(ProxSensor2Pin,INPUT);
  digitalWrite(ProxSensor2Pin,HIGH);  
  pinMode(EchoPin,INPUT);
  pinMode(EchoTriggerPin,OUTPUT);
  digitalWrite(EchoTriggerPin,LOW);
  pinMode(HeadLightsPin,OUTPUT);
  digitalWrite(HeadLightsPin,LOW);
  pinMode(HornPin,OUTPUT);
  digitalWrite(HornPin,LOW);
  pinMode(LeftLightPin,OUTPUT);
  digitalWrite(LeftLightPin,LOW);
  pinMode(RightLightPin,OUTPUT);
  digitalWrite(RightLightPin,LOW);
  pinMode(leftRearLightPin,OUTPUT);
  digitalWrite(leftRearLightPin,LOW);
  pinMode(rightRearLightPin,OUTPUT);
  digitalWrite(rightRearLightPin,LOW);
    
  if (digitalRead(ProxSensor1Pin)==LOW  || digitalRead(ProxSensor2Pin)==LOW) {
    useProxSensor=false;
    horn(1);
    delay(100);
    horn(0);
  } else {
    useProxSensor=true;
    horn(1);
    delay(100);
    horn(0);
    delay(100);
    horn(1);
    delay(100);
    horn(0);
  }

  Serial.begin(115200);
  // Load program from eeprom
  int i=0;
  c=EEPROM.read(i);
  while(c!=0) {
    pgrmString+=c;
    i++;
    c=EEPROM.read(i);
  }

  softSerial.begin(9600);
  
  // Timer  interrupt
  MsTimer2::set(50, timerISR); 
  MsTimer2::start();
}

void  loop() {
  while (Serial.available() > 0) {
    c = Serial.read();
    if  (c=='\n' || lineBuffer.length()==maxLineBuffer) {
      if (inputMode==1) {
        lineBuffer.trim();
        if (lineBuffer.compareTo("ep")==0) {
          // Save program to eeprom
          for (int i=0;i<pgrmString.length();i++)  EEPROM.write(i,pgrmString.charAt(i));
          EEPROM.write(pgrmString.length(),0);
          Serial.println("Program recorded : ");
          Serial.println(pgrmString);
          inputMode=0;
        } else {  
          if (!lineBuffer.endsWith(";")  && !lineBuffer.endsWith("]")) lineBuffer+=';';
          pgrmString=pgrmString+lineBuffer;
        }
      } else execPgrm(lineBuffer);
      lineBuffer="";
    }
    else lineBuffer+=c;
  }
  if (digitalRead(ButtonPin)==LOW) execPgrm(pgrmString);
}

void  moveForward() {
  PIDstate=1;
  PIDi=0;
  pwmL=pwmInit;
  pwmR=pwmInit;
  digitalWrite(MotorLDirectionPin,LOW);
  analogWrite(MotorLSpeedPin,pwmL);    
  digitalWrite(MotorRDirectionPin,LOW);
  analogWrite(MotorRSpeedPin,pwmR);  
}

void  moveBackward() {
  PIDstate=2;
  PIDi=0;
  digitalWrite(MotorLDirectionPin,HIGH);
  analogWrite(MotorLSpeedPin,255-pwmL);
  digitalWrite(MotorRDirectionPin,HIGH);
  analogWrite(MotorRSpeedPin,255-pwmR);
}

void turnRight() { 
  digitalWrite(MotorLDirectionPin,LOW);
  analogWrite(MotorLSpeedPin,rotPwm);
  digitalWrite(MotorRDirectionPin,HIGH);
  analogWrite(MotorRSpeedPin,255-rotPwm);
}

void turnLeft() {
  digitalWrite(MotorRDirectionPin,LOW);
  analogWrite(MotorRSpeedPin,rotPwm);
  digitalWrite(MotorLDirectionPin,HIGH);
  analogWrite(MotorLSpeedPin,255-rotPwm);
}

void stopMove() {
  PIDstate=0;
  digitalWrite(MotorRDirectionPin,LOW);
  digitalWrite(MotorRSpeedPin,LOW);
  digitalWrite(MotorLDirectionPin,LOW);
  digitalWrite(MotorLSpeedPin,LOW);
}

void  moveForward(int x) {
  stepCntTarget=x;
  stepCntL=0;
  stepCntR=0;
  PIDi=0;
  moveForward();  
  while(stepCntL<stepCntTarget && (!useProxSensor  || (digitalRead(ProxSensor1Pin) && digitalRead(ProxSensor2Pin)))) { 
    digitalWrite(EchoTriggerPin,  HIGH);
    delayMicroseconds(10);
    digitalWrite(EchoTriggerPin, LOW); 
    dist=pulseIn(EchoPin,HIGH,60000)/58;
    if (dist>0&&dist<20) break;
  }
  stopMove();
}

void moveBackward(int x) {
  stepCntTarget=x;
  stepCntL=0;
  stepCntR=0; 
  PIDi=0;
  moveBackward();
  while(stepCntL<stepCntTarget);  
  stopMove();
}

void turnLeft(int x) {
  stepCntTarget=x;
  stepCntL=0;
  stepCntR=0;   
  turnLeft();
  while(stepCntL<stepCntTarget);  
  stopMove();  
}

void turnRight(int x) {
  stepCntTarget=x;
  stepCntL=0;
  stepCntR=0;   
  turnRight();
  while(stepCntL<stepCntTarget);  
  stopMove();
}

void pause(int x) {
  delay(x);
}

void  setkc(int x) {
  Kc=double(x)/1000;
}

void setPwmInit(int x) {
  pwmInit=x;
}

void setspeed(int x) {
  speedTarget=x;
}

void  leftLight(int x) {
    if (x==0) {
      LeftLightState=false;
      digitalWrite(LeftLightPin,LOW);
    }
    else if (x==2) {
      LeftLightState=false;
      digitalWrite(LeftLightPin,HIGH);
    }
    else {
        digitalWrite(LeftLightPin,HIGH);
        LeftLightState=true;
        if (!RightLightState) SideLightCnt=0;
    }
}

void rightLight(int  x) {
    if (x==0) {
      RightLightState=false;
      digitalWrite(RightLightPin,LOW);
    }
    else if (x==2) {
      RightLightState=false;
      digitalWrite(RightLightPin,HIGH);
    }
    else {
      digitalWrite(RightLightPin,HIGH);
      RightLightState=true;
      if (!LeftLightState) SideLightCnt=0;
    }
    
}

void leftRearLight(int  x) {
  if (x==0) digitalWrite(leftRearLightPin,LOW);
    else digitalWrite(leftRearLightPin,HIGH);
}

void  rightRearLight(int x) {
  if (x==0) digitalWrite(rightRearLightPin,LOW);
    else digitalWrite(rightRearLightPin,HIGH);
}

void headLight(int x)  {
  if (x==0) digitalWrite(HeadLightsPin,LOW);
    else digitalWrite(HeadLightsPin,HIGH);
}

void  horn(int x) {
  if (x==0) digitalWrite(HornPin,LOW);
    else digitalWrite(HornPin,HIGH);
}

void  help() {
  Serial.println("fw (n) : move forward (by n steps)");
  Serial.println("bw  (n) : move backward (by n steps)");
  Serial.println("tl (n) : turn left (by  n steps)");
  Serial.println("tr (n) : turn right (by n steps)");
  Serial.println("st  : stop move");
  Serial.println("pa [n] : pause of n milliseconds");
  Serial.println("hl  [x] : head light [0: OFF, 1: ON]");
  Serial.println("ho [x] : horn [0: OFF,  1: ON]");
  Serial.println("lr [x] : left rear light [0: OFF, 1: ON]");
  Serial.println("rr [x] : right read light [0: OFF, 1: ON]");
  Serial.println("ll  [x] : left light [0: OFF, 1: CLIGN, 2:ON]");
  Serial.println("rl [x] : right  light [0: OFF, 1: CLIGN, 2:ON]");
  Serial.println("ss [n] : set speed");
  Serial.println("sp : save program");
  Serial.println("ep : end of program");
  Serial.println("rp : run program");
  Serial.println("pp : print program");
  Serial.println("ma.... : forward command ... to MeArm");
  Serial.println("he  : help");
}


void processCmd(String str) {
  Serial.print(str);
  Serial.print(" : ");

  int pos=str.indexOf(' ');
  boolean ok=1;
  
  if (str.startsWith("ma")) softSerial.println(str.substring(2)); // Forward  command to MeArm
  else {
    if (pos==-1) {  // command with no arg
      if  (str.compareTo("fw")==0 || str.compareTo("8")==0) moveForward();
      else  if (str.compareTo("bw")==0 || str.compareTo("2")==0) moveBackward();
      else  if (str.compareTo("tl")==0 || str.compareTo("4")==0) turnLeft();
      else  if (str.compareTo("tr")==0 || str.compareTo("6")==0) turnRight();
      else  if (str.compareTo("st")==0 || str.compareTo("5")==0) stopMove();
      else  if (str.compareTo("sp")==0) {pgrmString="";inputMode=1;}
      else if (str.compareTo("rp")==0)  execPgrm(pgrmString);
      else if (str.compareTo("pp")==0) Serial.println(pgrmString);
      else if (str.compareTo("he")==0) help();
      else ok=0;
    } else  {  // command with at least 1 arg
      String cmd=str.substring(0,pos);
      int arg=str.substring(pos+1).toInt();
      if (cmd.compareTo("fw")==0)  moveForward(arg);
      else if (cmd.compareTo("bw")==0) moveBackward(arg);
      else if (cmd.compareTo("tl")==0) turnLeft(arg);
      else if (cmd.compareTo("tr")==0)  turnRight(arg);
      else if (cmd.compareTo("pa")==0) pause(arg);
      else  if (cmd.compareTo("kc")==0) setkc(arg);    
      else if (cmd.compareTo("ss")==0)  setspeed(arg);        
      else if (cmd.compareTo("ll")==0) leftLight(arg);        
      else if (cmd.compareTo("rl")==0) rightLight(arg);
      else  if (cmd.compareTo("lr")==0) leftRearLight(arg);        
      else if (cmd.compareTo("rr")==0)  rightRearLight(arg);
      else if (cmd.compareTo("hl")==0) headLight(arg);
      else if (cmd.compareTo("ho")==0) horn(arg);
      else ok=0;
    }
  }
  if (ok) Serial.println("OK");
    else Serial.println("Error");
  if (inputMode==1) Serial.println("Recording program");
}

void execPgrm(String  str) {
  while(str.length()>0) {
    int endpos;
    str.trim();
    if  (str.startsWith("repeat")) {
      int startpos=str.indexOf('[');
      int  n=str.substring(6,startpos).toInt();
      int level=1;
      endpos=startpos;
      while(endpos<str.length()&&level>0) {
        endpos++;
        if (str.charAt(endpos)=='[')  level++;
        if (str.charAt(endpos)==']') level--;
      }
      for  (int i=0;i<n;i++) execPgrm(str.substring(startpos+1,endpos));
    } else {
      endpos=str.indexOf(';');
      if (endpos==-1) endpos=str.length();
      processCmd(str.substring(0,endpos));
    }
    if (endpos<str.length()-1)  str=str.substring(endpos+1);
      else str="";
  }
}

void ISR0()  {
  unsigned long intTime=micros();
  if ((intTime-prevIntTimeR)>IntDebounce  && digitalRead(IntPin0)==HIGH) {
    intPeriodR=intTime-prevIntTimeR;
    prevIntTimeR=intTime;
    stepCntR++;
  }
}

void ISR1() {
  unsigned long intTime=micros();
  if ((intTime-prevIntTimeL)>IntDebounce && digitalRead(IntPin1)==HIGH) {
    intPeriodL=intTime-prevIntTimeL;
    prevIntTimeL=intTime;
    stepCntL++;    
  }
}

void timerISR()  {
  if (PIDstate>0) {
    double target=speedTarget;
    if (PIDi<8) target=target*PIDi/8;
    double meas=3e6/double(intPeriodL);
    if (meas>1000) meas=0;
    double  error=target-meas;
    pwmL=pwmL+Kc*error;
    if (pwmL>255) pwmL=255;
    if (pwmL<0) pwmL=0;
    lasterrorL=error;
    if (PIDstate==2) analogWrite(MotorLSpeedPin,255-pwmL);
      else analogWrite(MotorLSpeedPin,pwmL);
  
    meas=3e6/double(intPeriodR);
    if (meas>1000) meas=0;
    error=target-meas;
    pwmR=pwmR+Kc*error;
    if (pwmR>255) pwmR=255;
    if (pwmR<0) pwmR=0;
    lasterrorR=error;
    if (PIDstate==2) analogWrite(MotorRSpeedPin,255-pwmR);
      else analogWrite(MotorRSpeedPin,pwmR);

    PIDi++;    
  }

  SideLightCnt++;
  if (LeftLightState && (SideLightCnt%10)==0)  digitalWrite(LeftLightPin,(SideLightCnt%20)==0);
  if (RightLightState && (SideLightCnt%10)==0)  digitalWrite(RightLightPin,(SideLightCnt%20)==0);
}

Downloadable files

MAX009 Schematic

Max009 Schematic PDF

MAX009 Schematic

MAX009 Schematic Fritzing source file

MAX009 Schematic

MAX009 Schematic Fritzing source file

MAX009 Schematic

MAX009 Schematic Fritzing source file

MAX009 Schematic

Comments

Only logged in users can leave comments

benoitdr

0 Followers

•

0 Projects

Intro