Pastebin launched a little side project called VERYVIRAL.com, check it out ;-) Want more features on Pastebin? Sign Up, it's FREE!

Cyclops 7.5

By: OllyR on Dec 18th, 2012  |  syntax: C++  |  size: 8.63 KB  |  views: 27  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1.  
  2. //Cyclops5: Hori and Vert, also: bring back the motors and indicators...
  3.  
  4. #include <AFMotor.h>       // Adafruit MotorShield
  5. #include <VarSpeedServo.h> // VarSpeedServo library gives more options, but variable speed isnt used yet
  6.  
  7. const int PR = A1; // connect PR to A1
  8. int Array[125];    // array to store sweep values
  9.  
  10. VarSpeedServo Hori;
  11. int HoriPos = 90; //  centre is 90. hardware modififed to prevent clashes. 30 (left) to 150 (right)
  12. int CWHoriDir = 0;
  13. int CCWHoriDir = 0;
  14.  
  15. VarSpeedServo Vert;
  16. int VertPos = 90; // 130 = level, 0 is back on itself, 30 is straight up
  17. int UpVertDir = 0;
  18. int DownVertDir = 0;
  19.  
  20. AF_DCMotor motorL(3, MOTOR12_1KHZ);  
  21. AF_DCMotor motorR(2, MOTOR12_1KHZ);
  22. const int RLED = 13; // indicator LED: 13 and 2 are the only properly unused digi pins on the motorshield.
  23. const int LLED = 2;  // indicator LED
  24.  
  25. 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.
  26. int HoriDelay;    // Steep (vertical) angle indicates closer to light and therefore moves in small steps to help prevent over run
  27.  
  28. // front sensors and integers for calibration
  29. const int LeftPR = A2;    // pin that the sensor is attached to
  30. const int CentrePR = A3;    // pin that the sensor is attached to
  31. const int RightPR = A4;    // pin that the sensor is attached to
  32. int LeftValue = 0;         // the sensor value
  33. int LeftMin = 1023;        // minimum sensor value
  34. int LeftMax = 0;           // maximum sensor value
  35. int CentreValue = 0;         // the sensor value
  36. int CentreMin = 1023;        // minimum sensor value
  37. int CentreMax = 0;           // maximum sensor value
  38. int RightValue = 0;         // the sensor value
  39. int RightMin = 1023;        // minimum sensor value
  40. int RightMax = 0;           // maximum sensor value
  41.  
  42. unsigned long previous = 0;
  43. unsigned long current = 0;
  44. unsigned long interval = 1000;
  45.  
  46.  
  47.  
  48.  
  49. void setup() { // Setup*******************
  50.  
  51.   Serial.begin(9600);  // Serial at 9600 bps
  52.   Hori.attach(9);           // 9 is "Servo2"
  53.   Vert.attach(10);          // 10 is "Servo1"
  54.   motorL.setSpeed(250);     // 0 is stop, 255 is full speed:
  55.   motorR.setSpeed(250);     //
  56.   pinMode (RLED,OUTPUT);    // indicator LEDS. HIGH is on, LOW is off
  57.   pinMode (LLED,OUTPUT);
  58.  
  59.  
  60.   // **************calibrate ************
  61.   {  
  62.     while (millis() < 5000) {
  63.  
  64.       //Spin on the spot
  65.     motorL.run(FORWARD);  
  66.     motorR.run(BACKWARD);  
  67.     digitalWrite(LLED, LOW);
  68.     digitalWrite(RLED, HIGH);
  69.  
  70.       //Calibrate
  71.       LeftValue = analogRead(LeftPR);
  72.       // record the maximum sensor value
  73.       if (LeftValue > LeftMax) {
  74.         LeftMax = LeftValue;
  75.       }
  76.       // record the minimum sensor value
  77.       if (LeftValue < LeftMin) {
  78.         LeftMin = LeftValue;
  79.       }
  80.       CentreValue = analogRead(CentrePR);
  81.       // record the maximum sensor value
  82.       if (CentreValue > CentreMax) {
  83.         CentreMax = CentreValue;
  84.       }
  85.       // record the minimum sensor value
  86.       if (CentreValue < CentreMin) {
  87.         CentreMin = CentreValue;
  88.       }
  89.       RightValue = analogRead(RightPR);
  90.       // record the maximum sensor value
  91.       if (RightValue > RightMax) {
  92.         RightMax = RightValue;
  93.       }
  94.       // record the minimum sensor value
  95.       if (RightValue < RightMin) {
  96.         RightMin = RightValue;
  97.       }
  98.     }
  99.  
  100.  
  101.     motorL.run(RELEASE);  
  102.     motorR.run(RELEASE);  
  103.     digitalWrite(LLED, LOW);
  104.     digitalWrite(RLED, LOW);
  105.  
  106.     // ***************end of the calibration******************
  107.  
  108.     BlindSpot();         //BlindSpot Check
  109.     unsigned long current = millis();
  110.   }
  111.  
  112. }
  113. void BlindSpot (){
  114.  
  115.   for(VertPos = 100; VertPos >= 1; VertPos -= 1)
  116.   {    
  117.     Array[VertPos-1] = analogRead(PR);
  118.     Vert.write(VertPos);
  119.     if ((Array[VertPos-1] < Array[VertPos]) && (VertPos<100)) {
  120.       UpVertDir = VertPos;
  121.     }
  122.     delay (10) ;      
  123.   }
  124.  
  125.   for(VertPos = 1; VertPos < 100; VertPos += 1)
  126.   {    
  127.     Array[VertPos-1] = analogRead(PR);
  128.     Vert.write(VertPos);
  129.     if ((Array[VertPos-1] < Array[VertPos+1]) && (VertPos>1)) {
  130.       DownVertDir = VertPos;
  131.     }
  132.     delay (10) ;      
  133.   }
  134.   VertPos = ((UpVertDir+DownVertDir)/2);   // Average the results to get the centre
  135.   Vert.write(VertPos);   // Go to the position and print the results
  136.   Serial.print("UpSweep: ");
  137.   Serial.print(UpVertDir);
  138.   Serial.print(", DownSweep: ");
  139.   Serial.print(DownVertDir);
  140.   Serial.print(", VertPos: ");
  141.   Serial.println(VertPos);
  142.   delay (100);
  143.  
  144.   Vert.write(100);
  145.  
  146.   if(VertPos < 50){  
  147.     motorL.run(FORWARD);  
  148.     motorR.run(BACKWARD);  
  149.     digitalWrite(LLED, LOW);
  150.     digitalWrite(RLED, HIGH);
  151.     delay (1750);
  152.     motorL.run(RELEASE);  
  153.     motorR.run(RELEASE);  
  154.     digitalWrite(LLED, LOW);
  155.     digitalWrite(RLED, LOW);
  156.  
  157.   }
  158. }
  159. void Decisions(){
  160.  
  161.  
  162.  
  163.   if((HoriPos > 110) && (HoriPos <= 150)){// single wheel moving
  164.     HoriDelay = map (HoriPos, 110, 150, 100,1000);
  165.  
  166.     if (current - previous < interval){
  167.     motorL.run(FORWARD);  
  168.     motorR.run(BACKWARD);  
  169.     digitalWrite(LLED, LOW);
  170.     digitalWrite(RLED, HIGH);
  171.   }
  172. else{
  173.     motorL.run(RELEASE);  
  174.     motorR.run(RELEASE);  
  175.     digitalWrite(LLED, LOW);
  176.     digitalWrite(RLED, LOW);
  177.  
  178.     }
  179.   }
  180.  
  181.  
  182.   //LEFT
  183.   if((HoriPos < 70) && (HoriPos >= 30)){ // Shallow angle results in single wheel moving
  184.     HoriDelay = map (HoriPos, 70, 30, 100, 1000);
  185.  
  186.   if (current - previous < interval){
  187.     motorL.run(BACKWARD);  
  188.     motorR.run(FORWARD);  
  189.     digitalWrite(LLED, HIGH);
  190.     digitalWrite(RLED, LOW);
  191.   }
  192. else{
  193.     motorL.run(RELEASE);  
  194.     motorR.run(RELEASE);  
  195.     digitalWrite(LLED, LOW);
  196.     digitalWrite(RLED, LOW);
  197.     }
  198.   }
  199.  
  200.   // FORWARD run forward: Window of 20 degrees in the centre results in running forward
  201.   if((HoriPos >= 70) && (HoriPos <= 110)){
  202.     VertDelay = map (VertPos, 130, 30, 10000, 500);
  203.      
  204.   if (current - previous < interval){
  205.     motorL.run(FORWARD);  
  206.     motorR.run(FORWARD);  
  207.     digitalWrite(LLED, HIGH);
  208.     digitalWrite(RLED, HIGH);
  209.  
  210.   }
  211.  
  212.     else{
  213.  
  214.     motorL.run(RELEASE);  
  215.     motorR.run(RELEASE);  
  216.     digitalWrite(LLED, LOW);
  217.     digitalWrite(RLED, LOW);
  218.  
  219.     }
  220.   }
  221.  
  222. }
  223. void HoriSweep (){ // Horizontal Sweep********
  224.  
  225.   for(HoriPos = 30; HoriPos < 150; HoriPos += 1)
  226.   {    
  227.     Array[HoriPos-30] = analogRead(PR);
  228.     Hori.write(HoriPos);
  229.     if ((Array[HoriPos-30] < Array[HoriPos-31]) && (HoriPos>30)) {
  230.       CWHoriDir = HoriPos;
  231.     }
  232.     delay (10) ;      
  233.   }
  234.   for(HoriPos = 150; HoriPos >= 30; HoriPos -= 1)
  235.   {    
  236.     Array[HoriPos-30] = analogRead(PR);
  237.     Hori.write(HoriPos);
  238.     if ((Array[HoriPos-30] < Array[HoriPos-29]) && (HoriPos<150)) {
  239.       CCWHoriDir = HoriPos;
  240.     }
  241.     delay (10) ;      
  242.   }
  243.   HoriPos = ((CWHoriDir+CCWHoriDir)/2); // Average the two value to get the centre
  244.   Hori.write(HoriPos); // Go to the position, and print the results to serial
  245.   Serial.print("CWSweep: ");
  246.   Serial.print(CWHoriDir);
  247.   Serial.print(", CCWSweep: ");
  248.   Serial.print(CCWHoriDir);
  249.   Serial.print(", HoriPos: ");
  250.   Serial.println(HoriPos);    
  251.   delay (500);
  252. }
  253.  
  254. void VertSweep (){ // Vertical Sweep*************
  255.  
  256.   for(VertPos = 130; VertPos >= 30; VertPos -= 1)
  257.   {    
  258.     Array[VertPos-30] = analogRead(PR);
  259.     Vert.write(VertPos);
  260.     if ((Array[VertPos-30] < Array[VertPos-29]) && (VertPos<130)) {
  261.       UpVertDir = VertPos;
  262.     }
  263.     delay (10) ;      
  264.   }
  265.  
  266.   for(VertPos = 30; VertPos < 130; VertPos += 1)
  267.   {    
  268.     Array[VertPos-30] = analogRead(PR);
  269.     Vert.write(VertPos);
  270.     if ((Array[VertPos-30] < Array[VertPos-31]) && (VertPos>30)) {
  271.       DownVertDir = VertPos;
  272.     }
  273.     delay (10) ;      
  274.   }
  275.   VertPos = ((UpVertDir+DownVertDir)/2);   // Average the results to get the centre
  276.   Vert.write(VertPos);   // Go to the position and print the results
  277.   Serial.print("UpSweep: ");
  278.   Serial.print(UpVertDir);
  279.   Serial.print(", DownSweep: ");
  280.   Serial.print(DownVertDir);
  281.   Serial.print(", VertPos: ");
  282.   Serial.println(VertPos);
  283.   delay (100);
  284.  
  285. }
  286.  
  287.  
  288. void loop() {
  289.  
  290.  
  291.   HoriPos = constrain (HoriPos, 30, 150); // constrain servos, Just in case.
  292.   VertPos = constrain (VertPos, 30, 130);
  293.   HoriSweep();        //Horizontal Scan
  294.   VertSweep();        // Vertical Scan
  295. //********************************
  296.   previous = current;
  297. //********************************
  298.  
  299.  
  300.    
  301.   //Stop under light
  302.   if(VertPos < 40){
  303.     motorL.run(RELEASE);  
  304.     motorR.run(RELEASE);  
  305.     digitalWrite(LLED, LOW);
  306.     digitalWrite(RLED, LOW);
  307.     delay (99999999); // lazy mans ending
  308.   }
  309.  
  310.   else {
  311.     Decisions();        // Decisions Decisions
  312.   }
  313. }