MyTurtle: self-learning robot (Part 2 - Movement unit)

1
2/* Basic robot car as a preparation for creating a self-learning robot
3  * by Viliam Krunka
4 */
5
6#include <Stepper.h>
7#include <Servo.h>
8
9#define  servoPin 3
10
11//motors
12const int stepsPerRevolution = 2048; 
13const int  rpm = 15;   //set speed of motors (max 16 for chosen step motors)
14const int cycle  = 500;
15
16Servo Head;
17Stepper LeftMotor = Stepper(stepsPerRevolution, 8,  10, 9, 11);
18Stepper RightMotor = Stepper(stepsPerRevolution, 4, 6, 5, 7);
19
20void  setup() {
21  InitializeHead();
22  InitializeMotors();
23}
24
25void loop()  {
26  switch(random(10)) {
27    case 0:
28      delay(1000);
29    case 1:
30      MoveForward(cycle);
31      break;
32    case 2:
33      MoveBack(cycle);
34      break;
35    case 3:
36      MoveLeftForward(cycle);
37      break;
38    case 4:
39      MoveRightForward(cycle);
40      break;
41    case 5:
42      TurnLeft(cycle);
43      break;
44    case 6:
45      TurnRight(cycle);
46      break;
47    case 7:
48      TurnHeadFront();
49      break;
50    case  8:
51      TurnHeadLeft();
52      break;
53    case 9:
54      TurnHeadRight();
55      break;
56  }
57}
58
59//servo motor
60
61void InitializeHead() {
62  Head.attach(servoPin);  //set pin & 
63}
64
65void TurnHeadFront() {
66  int  k = Head.read();
67  if (k < 90) {
68    for (k; k <= 90; k++) {
69      Head.write(k);   //set angle of servo motor  
70      delay(10);
71    }
72  } else {
73      for  (k; k >= 90; k--) {
74      Head.write(k);   //set angle of servo motor  
75      delay(10);
76    }
77  }
78}
79
80void TurnHeadLeft() {
81  for (int k = Head.read(); k  <= 150; k++) {
82    Head.write(k);   //set angle of servo motor  
83    delay(10);
84  }
85}
86
87void TurnHeadRight() {
88  for (int k = Head.read(); k >= 30; k--)  {
89    Head.write(k);   //set angle of servo motor
90    delay(10);
91  }
92}
93
94//motors
95
96void  InitializeMotors() {
97  LeftMotor.setSpeed(rpm);
98  RightMotor.setSpeed(rpm);
99}
100
101void  MoveForward(int cycle) {
102 for (int k=0; k < cycle; k++) {
103    LeftMotor.step(1);
104    RightMotor.step(1);
105 }
106}
107
108void MoveBack(int cycle) {
109 for (int  k=0; k < cycle; k++) {
110    LeftMotor.step(-1);
111    RightMotor.step(-1);
112  }
113}
114
115void MoveLeftForward(int cycle) {  //45 degrees = cycle 500
116 for  (int k=0; k < cycle; k++) {
117    LeftMotor.step(1);
118    RightMotor.step(3);
119  }
120}
121
122void MoveRightForward(int cycle) {  //45 degrees = cycle 500
123 for  (int k=0; k < cycle; k++) {
124    LeftMotor.step(3);
125    RightMotor.step(1);
126  }
127}
128
129void TurnLeft(int cycle) {    
130 for (int k=0; k < cycle; k++) {
131    LeftMotor.step(-1);
132    RightMotor.step(1);
133 }
134}
135
136void TurnRight(int  cycle) { 
137 for (int k=0; k < cycle; k++) {
138    LeftMotor.step(1);
139    RightMotor.step(-1);
140  }
141}