Control code for Chartreuse the moving robot
// SERVO | |
#include <Servo.h> | |
//NEOPIXELS | |
#include <Adafruit_NeoPixel.h> | |
//STEPPER | |
#include "Stepper_28BYJ_48.h" | |
#ifdef __AVR__ | |
#include <avr/power.h> | |
#endif | |
//set out signal pin | |
#define PIN 9 | |
Adafruit_NeoPixel strip = Adafruit_NeoPixel(2, PIN, NEO_GRB + NEO_KHZ800); | |
const int echo = 2; | |
const int trig = 3; | |
const int ultrasonicServoPin = 5; | |
const int ledStatusPin = 7; | |
const int eyeServoPin = 6; | |
const int ledPixelPin = 9; | |
Stepper_28BYJ_48 stepper(10,11,12,13); | |
Servo ultServo; | |
Servo eyeServo; | |
int ultAngle = 90; | |
int eyeAngle = 90; | |
int stepperAngle = 90; | |
long dur; | |
int dist; | |
long lastSeen; | |
void setup() { | |
pinMode(trig, OUTPUT); | |
pinMode(echo, INPUT); | |
pinMode(ledStatusPin, OUTPUT); | |
ultServo.attach(ultrasonicServoPin); //48deg -> 120 | |
eyeServo.attach(eyeServoPin); //0 -> 180 | |
ultServo.write(90); | |
eyeServo.write(90); | |
strip.begin(); | |
strip.show(); | |
Serial.begin(9600); // Starts the serial communication | |
} | |
void loop() { | |
/* | |
REMOTE CONTROL | |
int tester = analogRead(0); | |
int mapper = map(tester,0,1023,0,3); | |
setEyes(mapper); | |
if(digitalRead(1) == LOW){ | |
stepper.step(1); | |
} | |
if(digitalRead(2) == LOW){ | |
stepper.step(-1); | |
} | |
*/ | |
//MOVE SCANNER -> MOVE STEPPER -> MOVE EYES | |
int lastAngle = ultAngle; | |
// //ULTRASONIC TRACKING | |
getDistance(); | |
//Serial.println(dist); | |
if (dist > 10 && dist < 30){ //30 for testing | |
//Serial.println("Distance: " + String(dist) + " Angle: " + String(ultAngle)); | |
digitalWrite(ledStatusPin, HIGH); | |
if (ultAngle < 60){ | |
ultAngle = 60; | |
}else{ | |
ultAngle-=6; | |
} | |
}else{ | |
digitalWrite(ledStatusPin, LOW); | |
if (ultAngle > 120){ | |
ultAngle = 120; | |
}else{ | |
ultAngle++; | |
} | |
} | |
if (ultAngle > 50 && ultAngle < 108){ | |
lastSeen = millis(); | |
} | |
if (lastAngle != ultAngle){ | |
ultServo.write(ultAngle); | |
} | |
Serial.println(ultAngle); | |
delay(1); | |
//STEPPER ------------------------------------------ | |
stepToDeg(ultAngle); | |
delay(1); | |
//EYES --------------------------------------------- | |
long lastContact = (millis()-lastSeen); | |
//Serial.println(lastContact); | |
if (lastContact > 3000 && lastContact < 6000){ | |
setEyes(2); | |
}else if (lastContact <= 3000){ | |
setEyes(1); | |
}else{ | |
setEyes(0); | |
} | |
} | |
// ----------------------------------------------------------------------------------------- | |
void stepToDeg(int stepAngle){ | |
if (ultAngle < 121){ | |
int difference = stepperAngle - ultAngle; | |
int absDifference = abs(difference); | |
int negDif = -difference*7; | |
stepper.step(negDif); | |
stepperAngle = ultAngle; | |
} | |
} | |
void getDistance(){ //TAKES AN ULTRASONIC MEASUREMENT AND RETURNS THE COMPUTED DISTANCE IN CM | |
// Clears the trigPin | |
digitalWrite(trig, LOW); | |
delayMicroseconds(2); | |
//Sends pulse | |
digitalWrite(trig, HIGH); | |
delayMicroseconds(10); | |
digitalWrite(trig, LOW); | |
dur = pulseIn(echo, HIGH); | |
dist = dur*0.034/2; | |
} | |
void setEyes(int mode){ //SETS THE ANGLE OF THE EYE SERVO FOR VARIOUS EXPRESSIONS | |
switch (mode) { //DEFAULT:NEUTRAL 1:HAPPY 2:SAD | |
case 1: //HAPPY | |
eyeServo.write(179); | |
for(uint16_t i=0; i<strip.numPixels(); i++){ | |
strip.setPixelColor(i,255,140,0); | |
strip.show(); | |
} | |
break; | |
case 2: //SAD | |
eyeServo.write(1); | |
for(uint16_t i=0; i<strip.numPixels(); i++){ | |
strip.setPixelColor(i,90,90,255); | |
strip.show(); | |
} | |
break; | |
default: //NEUTRAL | |
eyeServo.write(99); | |
for(uint16_t i=0; i<strip.numPixels(); i++){ | |
strip.setPixelColor(i,255,200,180); | |
strip.show(); | |
} | |
break; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment