Draw Simple Shapes

Start 

This section will show off some code to get your robot to draw shapes. We are using timed instructions.

Circle Square Spiral 

Uncomment only the shape you want to draw. Make sure you're on fresh batteries or your results may vary.

Upload

Upload the following code.

Download file Copy to clipboard
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

//Libraries needed
#include "ServoDriver.h"
#include <Wire.h>
#include "Grove_I2C_Motor_Driver.h"

// default I2C address is 0x0f
#define I2C_ADDRESS 0x0f

//Creating the servo driver object
ServoDriver servoDriver;

//Motor names
#define LEFTMOTOR MOTOR2
#define RIGHTMOTOR MOTOR1

//PUT YOUR VALUES HERE
int minVal = 40;
int maxVal = 80;

//Shape to draw
String shape = "circle";
//String shape = "square";
//String shape = "spiral";

void setup() {
  //Open I2C communication
  Wire.begin();

  //Begin communication with driver
  servoDriver.init(0x7f);

  //Bring servo up to begin
  penUp();

  // start motor driver
  Motor.begin(I2C_ADDRESS);
  // correct pwm cycles
  Motor.frequence(F_490Hz);

}

void loop() {
  // put your main code here, to run repeatedly:
  //Wait to begin
  delay(10000);

  if (shape == "circle") {
    //Turn left motor on
    Motor.speed(LEFTMOTOR, 80);
    //Wait for Motor to hit speed
    delay(1000);
    //Put pen down
    penDown();
    //Draw circle
    delay(3300);
    //Stop motor
    Motor.stop(LEFTMOTOR);
    //Put pen up
    penUp();
  }

  if (shape == "square") {
    //One loop for each side
    for (int i = 0; i <= 3; i++) {
      //Put pen down
      penDown();
      //Set both motors forward
      Motor.speed(RIGHTMOTOR, -65);
      Motor.speed(LEFTMOTOR, 50);
      //Wait for distance
      delay(800);
      //Stop motors
      Motor.stop(RIGHTMOTOR);
      Motor.stop(LEFTMOTOR);
      //Wait
      delay(1000);
      // Lift up pen
      penUp();
      //Turn Right
      turnRight();
      //Set both motors Back
      Motor.speed(RIGHTMOTOR, 50);
      Motor.speed(LEFTMOTOR, -60);
      //Wait for distance
      delay(250);
      //Stop motors
      Motor.stop(RIGHTMOTOR);
      Motor.stop(LEFTMOTOR);
      //Wait
      delay(1000);
    }
    // Put pen up
    penUp();
  }

  if (shape == "spiral") {
    //Set right motor on
    Motor.speed(RIGHTMOTOR, 80);
    //Wait for motor to come up to speed
    delay(500);
    //n<=5 where 5 is the number of spirals
    for (int n = 0; n <= 2; n++) {
      // Put pen down
      penDown();
      //Wait for circle
      delay(3300);
      //Set left motor on
      Motor.speed(LEFTMOTOR, 50);
      //For a short time to nudge it
      delay(150);
      //Stop left motor
      Motor.stop(LEFTMOTOR);
    }
    //Stop right motor after spiral completed
    Motor.stop(RIGHTMOTOR);
    // Put pen up
    penUp();
  }

  //After shape is drawn stop forever
  while (1) {

  }
}

void penDown() {
  //Set servo to down position
  servoDriver.setAngle(1, minVal);
}

void penUp() {
  //Set servo to up position
  servoDriver.setAngle(1, maxVal);
}
void turnRight() {
  Motor.speed(LEFTMOTOR, 50);
  Motor.speed(RIGHTMOTOR, 50);

  delay(800);

  Motor.stop(LEFTMOTOR);
  Motor.stop(RIGHTMOTOR);
}

void turnLeft() {
  Motor.speed(LEFTMOTOR, -50);
  Motor.speed(RIGHTMOTOR, -50);

  delay(840);

  Motor.stop(LEFTMOTOR);
  Motor.stop(RIGHTMOTOR);
}

Observe

After five seconds the robot will have drawn a circle if you haven't changed anything above.

Common problems 

Here are some common problems and how to fix them.

The circle doesn't go fully around

Increase the second delay in the circle drawing section. Up from 3300.

The circle goes too much

Decrease the second delay in the circle drawing section. Down from 3300.

There is a weird mark when the pen goes down

This is normal.

The servo works but doesn't draw

Make sure your calibration variables are correct and added to the code.

The servo doesn't move

Check that both jumpers are still connected.