kids_robot_code

gistfile1.txt

// Download and install Trinket Pro drivers from
// https://learn.adafruit.com/introducing-pro-trinket/starting-the-bootloader

// [Tools] -> [Programmer] -> "USBtinyISP"
// [Tools] -> [Board] -> "Pro Trinket 3V/12 Mhz (USB)"


#include <Servo.h>

// setup servo
int servoPin = 8;
int PEN_DOWN = 170; // angle of servo when pen is down
int PEN_UP = 80;   // angle of servo when pen is up
Servo penServo;

int wheel_dia=64; //      # mm (increase = spiral out) 66.25
int wheel_base=112; //,    # mm (increase = spiral in) 
int steps_rev=512; //,     # 512 for 64x gearbox, 128 for 16x gearbox
int delay_time=6; //            # time between steps in ms

// Stepper sequence org->pink->blue->yel
int L_stepper_pins[] = {10, 12, 13, 11};
int R_stepper_pins[] = {3, 5, 6, 4};

int fwd_mask[][4] =  {{1, 0, 1, 0},
                      {0, 1, 1, 0},
                      {0, 1, 0, 1},
                      {1, 0, 0, 1}};

int rev_mask[][4] =  {{1, 0, 0, 1},
                      {0, 1, 0, 1},
                      {0, 1, 1, 0},
                      {1, 0, 1, 0}};


void setup() {
  randomSeed(analogRead(1)); 
  // put your setup code here, to run once:
  Serial.begin(9600);
  for(int pin=0; pin<4; pin++){
    pinMode(L_stepper_pins[pin], OUTPUT);
    digitalWrite(L_stepper_pins[pin], LOW);
    pinMode(R_stepper_pins[pin], OUTPUT);
    digitalWrite(R_stepper_pins[pin], LOW);
  }
  penServo.attach(servoPin);
  Serial.println("setup");
}


void loop(){ // draw a calibration box 4 times
  float length = 60;
  penup();
  pendown();
  curveRight(2.0 * 3.14 * 110);
 // drawAnE(length);
 // drawAD(length);
 // drawAnI(length);
 // drawAnE(length);
  //drawAnL();
  //drawATriangle();
  penup();
  done();      // releases stepper motor
  while(1);    // wait for reset
}

void drawATriangle() {
  pendown();
  forward(20);
  right(120);
  forward(20);
  right(120);
  forward(20);
 
}

void drawAnI(float length) {
  pendown();
  forward(length * (6.0/8.0));
  penup();
  forward(length * (1.0/8.0));
  pendown();
  forward(length * (1.0/8.0));
  penup();
  right(180);
  forward(length);
  // reset
  left(90);
  forward(length / 5.0);
  left(90);
  
}

void drawAD(float length) {
  pendown();
  forward(length);
  penup();
  right(120);
  pendown();
  forward(length / 2);
  penup();
  right(60);
  pendown();
  forward(length / 2);
  penup();
  right(60);
  pendown();
  forward(length/ 2);
  penup();
  // reset orientation
  left(150);
  forward((length /2) + length / 10);
  left(90);
}

void drawAnL(float length) {
  pendown();
  forward(length * (2/3));
  penup();
  right(90);
  pendown();
  forward(length);
  penup();
}

void drawAnE(float length) {
   float leg = length / 2;
   pendown();
   forward(length);
   right(90);
   forward(leg);
   penup();
   left(180);
   forward(leg);
   left(90);
   forward(leg);
   pendown();
   left(90);
   forward(leg);
   penup();
   left(180);
   forward(leg);
   left(90);
   forward(leg);
   left(90);
   pendown();
   forward(leg);
  penup();
  // reset orientation
  forward(length / 10);
  left(90);
}

void squares() {
    pendown();
  for(int x=0; x<12; x++){
    forward(100);
    penup();
    left(90);
    pendown();
  }
}

// ----- HELPER FUNCTIONS -----------
int step(float distance){
  int steps = distance * steps_rev / (wheel_dia * 3.1412); //24.61
  /*
  Serial.print(distance);
  Serial.print(" ");
  Serial.print(steps_rev);
  Serial.print(" ");  
  Serial.print(wheel_dia);
  Serial.print(" ");  
  Serial.println(steps);
  delay(1000);*/
  return steps;  
}


void forward(float distance){
  int steps = step(distance);
  Serial.println(steps);
  for(int step=0; step<steps; step++){
    for(int mask=0; mask<4; mask++){
      for(int pin=0; pin<4; pin++){
        digitalWrite(L_stepper_pins[pin], rev_mask[mask][pin]);
        digitalWrite(R_stepper_pins[pin], fwd_mask[mask][pin]);
      }
      delay(delay_time);
    } 
  }
}

void curveRight(float distance){
  int steps = step(distance);
  Serial.println(steps);
  for(int step=0; step<steps; step++){
    for(int mask=0; mask<4; mask++){
      for(int pin=0; pin<4; pin++){

        if ( step % 4 == 0 ) {
          digitalWrite(L_stepper_pins[pin], fwd_mask[mask][pin]);
        }
        
        digitalWrite(R_stepper_pins[pin], fwd_mask[mask][pin]);
      }
      delay(delay_time);
    } 
  }
}


void backward(float distance){
  int steps = step(distance);
  for(int step=0; step<steps; step++){
    for(int mask=0; mask<4; mask++){
      for(int pin=0; pin<4; pin++){
        digitalWrite(L_stepper_pins[pin], fwd_mask[mask][pin]);
        digitalWrite(R_stepper_pins[pin], rev_mask[mask][pin]);
      }
      delay(delay_time);
    } 
  }
}


void left(float degrees){
  float rotation = degrees / 360.0;
  float distance = wheel_base * 3.1412 * rotation;
  int steps = step(distance);
  for(int step=0; step<steps; step++){
    for(int mask=0; mask<4; mask++){
      for(int pin=0; pin<4; pin++){
        digitalWrite(R_stepper_pins[pin], rev_mask[mask][pin]);
        digitalWrite(L_stepper_pins[pin], rev_mask[mask][pin]);
      }
      delay(delay_time);
    } 
  }   
}


void right(float degrees){
  float rotation = degrees / 360.0;
  float distance = wheel_base * 3.1412 * rotation;
  int steps = step(distance);
  for(int step=0; step<steps; step++){
    for(int mask=0; mask<4; mask++){
      for(int pin=0; pin<4; pin++){
        digitalWrite(R_stepper_pins[pin], fwd_mask[mask][pin]);
        digitalWrite(L_stepper_pins[pin], fwd_mask[mask][pin]);
      }
      delay(delay_time);
    } 
  }   
}


void done(){ // unlock stepper to save battery
  for(int mask=0; mask<4; mask++){
    for(int pin=0; pin<4; pin++){
      digitalWrite(R_stepper_pins[pin], LOW);
      digitalWrite(L_stepper_pins[pin], LOW);
    }
    delay(delay_time);
  }
}


void pendown(){
  delay(250);
  Serial.println("PEN_UP()");
  penServo.write(PEN_UP);
  delay(250);
}


void penup(){
  delay(250);  
  Serial.println("PEN_DOWN()");
  penServo.write(PEN_DOWN);
  delay(250);
}