Squirrels have fuzzy tails.
0 Members and 1 Guest are viewing this topic.
Stuff will fry and kittens will cry
/* Mini Eric robot - using Roboduino - uses 2 modified servos for driving - uses 2 servos for a pan/tilt head - uses one Ping))) ultrasonic sensor mounted on the head - uses AVRcam for blob detection - uses one GP2D12 sensor mounted on a pan servo - uses 4 servos for arms - uses 1 servo for waist bend - uses a software serial port to talk to a serial LCD*/ //Includes#include <SoftwareSerial.h>#include <SoftwareServo.h>#include <AVRcam.h>//Sensors#define SharpPin 0 // analog pin 0#define PingPin 15 // digital pin 15 (analog 1)#define ClawSensor 16 // digital pin 16 (analog 2)//Servos#define LmotorPin 9 // digital pin 9, PWM pin#define RmotorPin 10 // digital pin 10, PWM pin#define BendPin 11 // digital pin 11, PWM pin#define LgripPin 12 // digital pin 12#define PanPin 4 // digital pin 4#define LrotatePin 5 // digital pin 5, PWM pin#define RrotatePin 6 // digital pin 6, PWM pin#define TiltPin 7 // digital pin 7#define RgripPin 8 // digital pin 8Servo Lmotor;Servo Rmotor;Servo Pan;Servo Tilt;//Media#define Led 13 //digital pin 13 #define Speaker 17 //digital pin 17 (analog 3)#define rxPin 3#define txPin 2SoftwareSerial mySerial = SoftwareSerial(rxPin, txPin);//Variablesunsigned int PingTime = 0;byte center = 90;int dx=0, dy=0;byte xMid = 144/2;byte yMid = 88/2;byte num = 0;//----------------------------------------------------------------void setup() { delay(1000); //wait 1 second before start for debug purposes //AVRcam initialisation - initializes the hardware serial at 115200 bps camIni(1, 1, 1, Servo::refresh); //sensors pinMode(PingPin, OUTPUT); digitalWrite(PingPin, LOW); //head servos Pan.attach(PanPin); Tilt.attach(TiltPin); //software serial for LCD pinMode(rxPin, INPUT); pinMode(txPin, OUTPUT); mySerial.begin(9600); mySerial.print("AVRcam test"); // print in the first LCD line mySerial.print(2, BYTE); // print in the second LCD line mySerial.print(5, BYTE); // start from the fifth position mySerial.print("robot"); // text to print delay(100); //wait a little Clear_LCD(); Pan.write(center); Tilt.write(center); Servo::refresh();}//================================================================void loop(){ Read_Camera(); Pan_Tilt(); Servo::refresh(); delay(100); Read_Ping_Sensor();} int Read_Ping_Sensor() { //trigger the sensor pinMode(PingPin, OUTPUT); digitalWrite(PingPin, LOW); delayMicroseconds(2); digitalWrite(PingPin, HIGH); delayMicroseconds(5); digitalWrite(PingPin, LOW); //receive the echo pinMode(PingPin, INPUT); digitalWrite(PingPin, HIGH); // turn on pull up resistor PingTime = pulseIn(PingPin, HIGH); mySerial.print(1, BYTE); // print in the first LCD line mySerial.print(1, BYTE); // start from the first position mySerial.print("Ping: "); // text to print mySerial.print(PingTime, DEC); // variable to print return PingTime;}void Read_Camera() { byte obj; obj = camRead(); num = camObjNum(); if (num >0) { Process_Objects(); } switch (obj) { //case 0: // obj found //Process_Objects(); case 1: // comunication error mySerial.print(2, BYTE); // print in the second LCD line mySerial.print(1, BYTE); // start from the first position mySerial.print("comm error "); case 2: // obj not found mySerial.print(2, BYTE); // print in the second LCD line mySerial.print(1, BYTE); // start from the first position mySerial.print("no obj found "); } }int Process_Objects() { dx=0; dy=0; byte size = 0; byte i, s, ox, oy; for (byte j; j < num; j++) { s = camObjSize(j); if (s > size){ size = s; i = j; } } ox = camObjX(i); // range: 0-144 oy = camObjY(i); // range: 0-88 dx = ox - xMid; dy = oy - yMid; Clear_LCD(); mySerial.print(2, BYTE); // print in the second LCD line mySerial.print(1, BYTE); // start from the first position mySerial.print("dX: "); // text to print mySerial.print(dx, DEC); // variable to print mySerial.print(" dY: "); // text to print mySerial.print(dy, DEC); // variable to print return dx; return dy;}void Pan_Tilt() { int x = dx/8; int y = dy/6; Pan.write(center - x); Tilt.write(center + y); }void Clear_LCD() { mySerial.print(5, BYTE); // select LCD command mode mySerial.print(1, BYTE); // clear screen}