Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //Cyclops5: Hori and Vert, also: bring back the motors and indicators...
- #include <AFMotor.h> // Adafruit MotorShield
- #include <VarSpeedServo.h> // VarSpeedServo library gives more options, but variable speed isnt used yet
- const int PR = A1; // connect PR to A1
- int Array[125]; // array to store sweep values
- VarSpeedServo Hori;
- int HoriPos = 90; // centre is 90. hardware modififed to prevent clashes. 30 (left) to 150 (right)
- int CWHoriDir = 0;
- int CCWHoriDir = 0;
- VarSpeedServo Vert;
- int VertPos = 90; // 130 = level, 0 is back on itself, 30 is straight up
- int UpVertDir = 0;
- int DownVertDir = 0;
- AF_DCMotor motorL(3, MOTOR12_1KHZ);
- AF_DCMotor motorR(2, MOTOR12_1KHZ);
- const int RLED = 13; // indicator LED: 13 and 2 are the only properly unused digi pins on the motorshield.
- const int LLED = 2; // indicator LED
- int VertDelay; // integer used to calculate "distance" to travel. Shallow angle on Vert indicates distance to light is greater,and increases time to run motors.
- int HoriDelay; // Steep (vertical) angle indicates closer to light and therefore moves in small steps to help prevent over run
- // front sensors and integers for calibration
- const int LeftPR = A2; // pin that the sensor is attached to
- const int CentrePR = A3; // pin that the sensor is attached to
- const int RightPR = A4; // pin that the sensor is attached to
- int LeftValue = 0; // the sensor value
- int LeftMin = 1023; // minimum sensor value
- int LeftMax = 0; // maximum sensor value
- int CentreValue = 0; // the sensor value
- int CentreMin = 1023; // minimum sensor value
- int CentreMax = 0; // maximum sensor value
- int RightValue = 0; // the sensor value
- int RightMin = 1023; // minimum sensor value
- int RightMax = 0; // maximum sensor value
- unsigned long previous = 0;
- unsigned long current = 0;
- unsigned long interval = 1000;
- void setup() { // Setup*******************
- Serial.begin(9600); // Serial at 9600 bps
- Hori.attach(9); // 9 is "Servo2"
- Vert.attach(10); // 10 is "Servo1"
- motorL.setSpeed(250); // 0 is stop, 255 is full speed:
- motorR.setSpeed(250); //
- pinMode (RLED,OUTPUT); // indicator LEDS. HIGH is on, LOW is off
- pinMode (LLED,OUTPUT);
- // **************calibrate ************
- {
- while (millis() < 5000) {
- //Spin on the spot
- motorL.run(FORWARD);
- motorR.run(BACKWARD);
- digitalWrite(LLED, LOW);
- digitalWrite(RLED, HIGH);
- //Calibrate
- LeftValue = analogRead(LeftPR);
- // record the maximum sensor value
- if (LeftValue > LeftMax) {
- LeftMax = LeftValue;
- }
- // record the minimum sensor value
- if (LeftValue < LeftMin) {
- LeftMin = LeftValue;
- }
- CentreValue = analogRead(CentrePR);
- // record the maximum sensor value
- if (CentreValue > CentreMax) {
- CentreMax = CentreValue;
- }
- // record the minimum sensor value
- if (CentreValue < CentreMin) {
- CentreMin = CentreValue;
- }
- RightValue = analogRead(RightPR);
- // record the maximum sensor value
- if (RightValue > RightMax) {
- RightMax = RightValue;
- }
- // record the minimum sensor value
- if (RightValue < RightMin) {
- RightMin = RightValue;
- }
- }
- motorL.run(RELEASE);
- motorR.run(RELEASE);
- digitalWrite(LLED, LOW);
- digitalWrite(RLED, LOW);
- // ***************end of the calibration******************
- BlindSpot(); //BlindSpot Check
- unsigned long current = millis();
- }
- }
- void BlindSpot (){
- for(VertPos = 100; VertPos >= 1; VertPos -= 1)
- {
- Array[VertPos-1] = analogRead(PR);
- Vert.write(VertPos);
- if ((Array[VertPos-1] < Array[VertPos]) && (VertPos<100)) {
- UpVertDir = VertPos;
- }
- delay (10) ;
- }
- for(VertPos = 1; VertPos < 100; VertPos += 1)
- {
- Array[VertPos-1] = analogRead(PR);
- Vert.write(VertPos);
- if ((Array[VertPos-1] < Array[VertPos+1]) && (VertPos>1)) {
- DownVertDir = VertPos;
- }
- delay (10) ;
- }
- VertPos = ((UpVertDir+DownVertDir)/2); // Average the results to get the centre
- Vert.write(VertPos); // Go to the position and print the results
- Serial.print("UpSweep: ");
- Serial.print(UpVertDir);
- Serial.print(", DownSweep: ");
- Serial.print(DownVertDir);
- Serial.print(", VertPos: ");
- Serial.println(VertPos);
- delay (100);
- Vert.write(100);
- if(VertPos < 50){
- motorL.run(FORWARD);
- motorR.run(BACKWARD);
- digitalWrite(LLED, LOW);
- digitalWrite(RLED, HIGH);
- delay (1750);
- motorL.run(RELEASE);
- motorR.run(RELEASE);
- digitalWrite(LLED, LOW);
- digitalWrite(RLED, LOW);
- }
- }
- void Decisions(){
- if((HoriPos > 110) && (HoriPos <= 150)){// single wheel moving
- HoriDelay = map (HoriPos, 110, 150, 100,1000);
- if (current - previous < interval){
- motorL.run(FORWARD);
- motorR.run(BACKWARD);
- digitalWrite(LLED, LOW);
- digitalWrite(RLED, HIGH);
- }
- else{
- motorL.run(RELEASE);
- motorR.run(RELEASE);
- digitalWrite(LLED, LOW);
- digitalWrite(RLED, LOW);
- }
- }
- //LEFT
- if((HoriPos < 70) && (HoriPos >= 30)){ // Shallow angle results in single wheel moving
- HoriDelay = map (HoriPos, 70, 30, 100, 1000);
- if (current - previous < interval){
- motorL.run(BACKWARD);
- motorR.run(FORWARD);
- digitalWrite(LLED, HIGH);
- digitalWrite(RLED, LOW);
- }
- else{
- motorL.run(RELEASE);
- motorR.run(RELEASE);
- digitalWrite(LLED, LOW);
- digitalWrite(RLED, LOW);
- }
- }
- // FORWARD run forward: Window of 20 degrees in the centre results in running forward
- if((HoriPos >= 70) && (HoriPos <= 110)){
- VertDelay = map (VertPos, 130, 30, 10000, 500);
- if (current - previous < interval){
- motorL.run(FORWARD);
- motorR.run(FORWARD);
- digitalWrite(LLED, HIGH);
- digitalWrite(RLED, HIGH);
- }
- else{
- motorL.run(RELEASE);
- motorR.run(RELEASE);
- digitalWrite(LLED, LOW);
- digitalWrite(RLED, LOW);
- }
- }
- }
- void HoriSweep (){ // Horizontal Sweep********
- for(HoriPos = 30; HoriPos < 150; HoriPos += 1)
- {
- Array[HoriPos-30] = analogRead(PR);
- Hori.write(HoriPos);
- if ((Array[HoriPos-30] < Array[HoriPos-31]) && (HoriPos>30)) {
- CWHoriDir = HoriPos;
- }
- delay (10) ;
- }
- for(HoriPos = 150; HoriPos >= 30; HoriPos -= 1)
- {
- Array[HoriPos-30] = analogRead(PR);
- Hori.write(HoriPos);
- if ((Array[HoriPos-30] < Array[HoriPos-29]) && (HoriPos<150)) {
- CCWHoriDir = HoriPos;
- }
- delay (10) ;
- }
- HoriPos = ((CWHoriDir+CCWHoriDir)/2); // Average the two value to get the centre
- Hori.write(HoriPos); // Go to the position, and print the results to serial
- Serial.print("CWSweep: ");
- Serial.print(CWHoriDir);
- Serial.print(", CCWSweep: ");
- Serial.print(CCWHoriDir);
- Serial.print(", HoriPos: ");
- Serial.println(HoriPos);
- delay (500);
- }
- void VertSweep (){ // Vertical Sweep*************
- for(VertPos = 130; VertPos >= 30; VertPos -= 1)
- {
- Array[VertPos-30] = analogRead(PR);
- Vert.write(VertPos);
- if ((Array[VertPos-30] < Array[VertPos-29]) && (VertPos<130)) {
- UpVertDir = VertPos;
- }
- delay (10) ;
- }
- for(VertPos = 30; VertPos < 130; VertPos += 1)
- {
- Array[VertPos-30] = analogRead(PR);
- Vert.write(VertPos);
- if ((Array[VertPos-30] < Array[VertPos-31]) && (VertPos>30)) {
- DownVertDir = VertPos;
- }
- delay (10) ;
- }
- VertPos = ((UpVertDir+DownVertDir)/2); // Average the results to get the centre
- Vert.write(VertPos); // Go to the position and print the results
- Serial.print("UpSweep: ");
- Serial.print(UpVertDir);
- Serial.print(", DownSweep: ");
- Serial.print(DownVertDir);
- Serial.print(", VertPos: ");
- Serial.println(VertPos);
- delay (100);
- }
- void loop() {
- HoriPos = constrain (HoriPos, 30, 150); // constrain servos, Just in case.
- VertPos = constrain (VertPos, 30, 130);
- HoriSweep(); //Horizontal Scan
- VertSweep(); // Vertical Scan
- //********************************
- previous = current;
- //********************************
- //Stop under light
- if(VertPos < 40){
- motorL.run(RELEASE);
- motorR.run(RELEASE);
- digitalWrite(LLED, LOW);
- digitalWrite(RLED, LOW);
- delay (99999999); // lazy mans ending
- }
- else {
- Decisions(); // Decisions Decisions
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement