Project-Alex/robotlib.ino at master · KishorKumar11/Project-Alex · 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
// Direction values
typedef enum dir
{
  STOP,
  FORWARD,
  BACKWARD,
  LEFT,
  RIGHT
} dir;

// Motor control pins
#define M1F   5
#define M1R   6
#define M2F   9
#define M2R   10


// Ensure that percent is between 0 and 100
double checkPercent(double percent)
{
  if(percent < 0.0)
    return 0.0;
  else
    if(percent > 100.0)
      return 100.0;
    else
      return percent;
}

// Get PWM value based on percentage
int getPWMValue(double percent)
{
  return (int)(percent/100.0 * 255.0);
}

void move(double percent, int direction)
{
  int val=getPWMValue(checkPercent(percent));

  // Forward and reverse values for motors 1 and 2
  int M1FVal, M1RVal, M2FVal, M2RVal;

  switch(direction)
  {
    case FORWARD:
      M1FVal=val;
      M2FVal=val;

      M1RVal=0;
      M2RVal=0;

    break;

    case BACKWARD:
      M1FVal=0;
      M2FVal=0;

      M1RVal=val;
      M2RVal=val;

    break;

    case LEFT:
      M1FVal=0;
      M2FVal=val;
      M1RVal=val;
      M2RVal=0;
    break;

    case RIGHT:
     M1FVal=val;
     M2FVal=0;
     M1RVal=0;
     M2RVal=val;
    break;

    case STOP:
    default:

    M1FVal=0;
    M2FVal=0;
    M1RVal=0;
    M2RVal=0;
  }

  analogWrite(M1F, M1FVal);
  analogWrite(M2F, M2FVal);
  analogWrite(M1R, M1RVal);
  analogWrite(M2R, M2RVal);
}

void forward(double percent)
{
  move(percent, FORWARD);
}

void backward(double percent)
{
  move(percent, BACKWARD);
}

void left(double percent)
{
  move(percent, LEFT);
}

void right(double percent)
{
  move(percent, RIGHT);
}

void stop()
{
  move(0.0, STOP);
}