document.write('
Data hosted with ♥ by Pastebin.com - Download Raw - See Original
  1. //NEW
  2. //ignores ultra short rangefinder detections (expressed in RFTHRESH)
  3. //stops for around 1 second in case of collision
  4. //TODO
  5. //make tinah board emit sounds of different frequency depending on servo angle, for ease of testing & analysis.
  6.  
  7. //TO BE REMOVED BEFORE COMPETITION:
  8. //everything that is printed to screen and associated things. We will even remove the screen probably.
  9. //3-second pause.
  10.  
  11. #include <phys253.h>        //   ***** from 253 template file
  12. #include <LiquidCrystal.h>  //   ***** from 253 template file
  13. #include <servo253.h>       //   ***** from 253 template file
  14.  
  15. #define LOOP2 0
  16. #define LOOP8 1 //for readability
  17. #define TRACKIN 1
  18. #define TRACKOUT 0
  19. #define CENTER 90
  20. #define RFTHRESH 179
  21. #define ZZTIME 150
  22. #define STOP_THRESH 900
  23. #define STOP_TIME 1111
  24.  
  25. int MIN_ANGLE=CENTER-39;
  26. int MAX_ANGLE=CENTER+39;
  27.  
  28. int menu=0;
  29.  
  30. int s_ref8 = 11;
  31. int s_ref = 11;
  32. //int ref_angle[8] = {0, 12, 24, 36, 48, 60, 72, 84};
  33. int var_angle;
  34. int ref_angle[8] = {0, 3, 5, 9, 12, 16, 21, 37};
  35. int angle=CENTER;
  36. long f_angle=CENTER;//this goes into SERVO
  37. long f_angle0=CENTER;//the old f_angle;
  38. int p_angle[15];//is assigned later from ref_angle
  39.  
  40. long dt; long t0; long t1; long kd=10; long der_angle=0;
  41.  
  42. boolean loop28 = LOOP8;//TRUE IF RUNNING LOOP8, FALSE IF RUNNING LOOP2. starts with LOOP8 by default
  43. boolean loopmotor = 1;
  44.  
  45. int motorSpeed = 1023;
  46. const int BRAKE_CONSTANT=200;//milliseconds to brake!
  47. const int ACCEL_CONSTANT=210 ;//milliseconds to accelerate after braking! (note, the amount of time it accelerates is this minus the BRAKE_CONSTANT)
  48. const int motorSpeedCurve[15]={400,480,560,640,720,800,880,1023,880,800,720,640,560,480,400};
  49. int curveFactor = 0;//MUST BE BETWEEN 0 and 14 (inclusive).
  50. //const int curveSounds[16]={278,294,312,330,350,371,393,416,441,467,495,524,556,589,624,661};
  51.  
  52. const int numIterations=3300;//The higher this is, the smoother the steering.
  53. long tStart;//For testing how frequently the robot scans the tape.
  54. long tRun;//time counter for motor speed control.
  55.  
  56. boolean inner=TRACKOUT;//Is it on the inner track?
  57. boolean lanechange=0;    //Do we want to change lanes?
  58. int lanestage=0;
  59. long lanetime;
  60. boolean lanet=0;
  61. boolean laneon;//have we left original lane
  62. int lanelastsen;//last sensor
  63. int delaytime=133;//for lanechanging
  64.  
  65. long zztime;
  66. boolean zztimer=0;
  67.  
  68. long i_angle=0;
  69. long ki=0;
  70. boolean iord;//buttons/knob set ki or kd: I true D false (T or F = I or D)
  71. int knob70;//previous knob7 value; only print knob(7)!=knob70
  72.  
  73.  
  74. void setup(){
  75.   portMode(0, INPUT); portMode(1, INPUT);      //   ***** from 253 template file
  76.   RCServo0.attach(RCServo0Output);RCServo1.attach(RCServo1Output);RCServo2.attach(RCServo2Output);
  77.   RCServo0.write(CENTER);
  78.  
  79.   while(!startbutton()&&!stopbutton()){
  80.     LCD.clear(); LCD.home();
  81.     LCD.print("STOP br START se");
  82.     LCD.setCursor(0,1);
  83.     for(int i=8;i<=15;i++){
  84.       if(digitalRead(i)) LCD.print("1");
  85.       else               LCD.print("0");
  86.     }
  87.     delay(200);
  88.   }
  89.   while(startbutton()||stopbutton()){delay(100);}
  90.   MIN_ANGLE=CENTER-39;
  91.   MAX_ANGLE=CENTER+39;
  92.   while(!startbutton()){//WE KEEP THIS INNER//OUTER SETUP
  93.     LCD.clear(); LCD.home();
  94.     if(inner==TRACKOUT) LCD.print("OUTER LANE?");
  95.     else LCD.print("INNER LANE?");
  96.     if(stopbutton()){
  97.       while(stopbutton()){delay(100);}
  98.       if(inner==TRACKIN){
  99.         inner=TRACKOUT;
  100.         digitalWrite(31,HIGH);
  101.       }
  102.       else{
  103.         inner=TRACKIN;
  104.         digitalWrite(33,HIGH);
  105.       }
  106.     }
  107.     delay(300);
  108.   }
  109.   while(startbutton()){delay(100);}
  110.   while(!startbutton()){
  111.     if(stopbutton()){
  112.       while(stopbutton()){delay(100);}
  113.       if(menu!=2) menu++;
  114.       else menu=0;
  115.     }
  116.     LCD.clear(); LCD.home();
  117.     LCD.print("Which angles?");
  118.     LCD.setCursor(0,1);
  119.     if(menu==0) LCD.print("PRESET IN CODE->");
  120.     else if(menu==1) LCD.print("CUSTOM ANGLES-->");
  121.     else LCD.print("PROPORTIONAL--->");
  122.     delay(200);
  123.   }
  124.   while(startbutton()){delay(100);}
  125.   LCD.clear(); LCD.home();
  126.   if(menu==0){}
  127.   else if (menu==1){
  128.     for(int i=0;i<=7;i++){//set the angles using knob 6.
  129.       while(!startbutton()){
  130.         if(stopbutton()){//exit
  131.           while(stopbutton()) delay(100);
  132.           i=8;
  133.           break;
  134.         }
  135.         LCD.clear(); LCD.home();
  136.         LCD.print("Angle #"); LCD.print(i); LCD.print(" is ");
  137.         var_angle=knob(6)*9/100;
  138.         LCD.print(var_angle);
  139.         LCD.setCursor(0,1);
  140.         LCD.print("(turn knob 6)");
  141.         delay(100);
  142.       }
  143.       ref_angle[i]=var_angle;
  144.       while(startbutton()){delay(100);}//so you don\'t skip
  145.     }
  146.   }else{
  147.     ref_angle[0]=0;
  148.     while(!startbutton()){
  149.         LCD.clear(); LCD.home();
  150.         LCD.print("INTERVAL: ");
  151.         var_angle=knob(6)*9/100;
  152.         LCD.print(var_angle);
  153.         LCD.setCursor(0,1);
  154.         LCD.print("(turn knob 6)");
  155.         delay(100);
  156.     }
  157.     for(int i=1;i<=7;i++){
  158.       ref_angle[i]=var_angle*i;
  159.     }
  160.     while(startbutton()){delay(100);}
  161.   }
  162.  
  163.   delay(100);
  164.  
  165.   //ASSIGN ANGLES FROM REFERENCE
  166.   for(int i=0;i<=7;i++){
  167.     p_angle[i+7]=ref_angle[i]+CENTER;
  168.     p_angle[7-i]=-ref_angle[i]+CENTER;
  169.   }
  170.  
  171.   LCD.clear(); LCD.home();
  172.   LCD.print("ANGLES ");//Prints out all angles on the screen.
  173.   LCD.print(p_angle[0]); LCD.print(" "); LCD.print(p_angle[1]); LCD.print(" ");
  174.   LCD.print(p_angle[2]); LCD.setCursor(0,1); LCD.print(p_angle[3]); LCD.print(" ");
  175.   LCD.print(p_angle[4]); LCD.print(" "); LCD.print(p_angle[5]); LCD.print(" ");
  176.   LCD.print(p_angle[6]); LCD.print(" "); LCD.print(p_angle[7]);
  177.  
  178.   while(startbutton()){delay(100);}
  179.   while(!startbutton()){delay(100);}
  180.   while(startbutton()){delay(100);}
  181.  
  182.   LCD.clear(); LCD.home();
  183.   LCD.setCursor(0,1);
  184.   LCD.print("Sensor Check");
  185.   while(!startbutton()){
  186.     LCD.home();
  187.     for(int i=8;i<=15;i++){
  188.       if(digitalRead(i)) LCD.print("1");
  189.       else               LCD.print("0");
  190.     }
  191.     delay(50);
  192.   }
  193.   LCD.clear(); LCD.home();
  194.   LCD.print("STARTING IN:");
  195.   LCD.setCursor(0,1); LCD.print("3"); delay(300);
  196.   LCD.setCursor(0,1); LCD.print("2"); delay(300);
  197.   LCD.setCursor(0,1); LCD.print("1"); delay(300);
  198.   LCD.clear();LCD.home();
  199.   tRun=millis();
  200. }
  201.  
  202. void loop(){
  203.   for(int j=1;j<=numIterations;j++){
  204.     tStart=micros();
  205.     if(lanechange==0){
  206.       if(loop28){                     //IN LOOP8 MODE
  207.         //THINGS THAT DON"T WORK ARE COMMENTED OUT!!!
  208.         /*if(s_ref8==15) s_ref8=8;
  209.         else s_ref8++;
  210.         if(digitalRead(s_ref8)){
  211.           //ACCIDENTAL LANE CHANGE
  212.          
  213.           //if(last sensor WAS on left, it WAS on the inner track, and it is NOW on right, it becomes outer)
  214.           //or(last sensor WAS right, OUTER track, now on LEFT. it must be on inner
  215.           if(s_ref<=11&&inner==TRACKIN&&s_ref8>=12||s_ref>=12&&inner==TRACKOUT&&s_ref8<=11)
  216.           inner=!inner;
  217.          
  218.           if(s_ref8==15) s_ref8=14;
  219.          
  220.           s_ref=s_ref8;
  221.          
  222.           loop28=LOOP2;               //If something is detected, loop on 2!!
  223.          
  224.         }*/
  225.         if(s_ref==15) s_ref=8;
  226.         else s_ref++;
  227.         if(digitalRead(s_ref)){
  228.           if(s_ref==15) s_ref=14;
  229.           loop28=LOOP2;               //If something is detected, loop on 2!!
  230.         }
  231.       }else{                           //IN LOOP2 MODE
  232.         if(digitalRead(s_ref)){
  233.           zztime=0;
  234.           if(digitalRead(s_ref+1)){   //If both relevant sensors are on
  235.             curveFactor=2*(s_ref-8)+1;
  236.             angle=p_angle[curveFactor];
  237.           }else{                      //If 10, shift s_ref once leftwards.
  238.             curveFactor=2*(s_ref-8);
  239.             angle=p_angle[curveFactor];
  240.             if(s_ref>10) s_ref--;
  241.           }
  242.         }else{
  243.           if(digitalRead(s_ref+1)){   //If 01, shift s_ref once rightwards.
  244.             curveFactor=2*(s_ref-8)+2;
  245.             angle=p_angle[curveFactor];
  246.             if(s_ref<12) s_ref++;
  247.             zztime=0;
  248.           }else{                      //If nothing is detected, loop on 8!!
  249.             if(zztime==0){
  250.               zztimer=millis();
  251.               zztime=1;}
  252.               else
  253.               if(millis()-zztimer>ZZTIME){
  254.                 zztime=0;
  255.                 loop28=LOOP8;
  256.                 s_ref=8;// Start loop8 from the left, so that it doesn\'t get distracted by markers.
  257.               }
  258.           }
  259.         }
  260.         t1=micros();  //Use t1 so that you don\'t have to call micros() twice - faster and more precise
  261.         dt=t1-t0; t0=t1; //DT is about 100
  262.         der_angle=kd*(f_angle-f_angle0)/dt;
  263.         //i_angle+=((long)(f_angle-CENTER)*dt*ki);//DT = 100, KI is from 0 to 1000
  264.         //if(i_angle>10) i_angle=10;
  265.         //if(i_angle<-10) i_angle=-10;
  266.         f_angle=angle-der_angle;//-i_angle;
  267.        
  268.         if(f_angle<MIN_ANGLE) f_angle=MIN_ANGLE; //0<=angle<=180
  269.         if(f_angle>MAX_ANGLE) f_angle=MAX_ANGLE;
  270.         if(f_angle!=f_angle0){
  271.           RCServo0.write(f_angle);
  272.           if(abs(f_angle-CENTER)>10&&abs(f_angle-f_angle0)>15){
  273.             tRun=millis();
  274.           }
  275.         }
  276.         f_angle0=f_angle;
  277.       }
  278.     }else{//LANE CHANGE PROCESS BEGINS
  279.       if(lanestage==0){//Stage 0. Set the initial angle (left if on outer track, right if on inner track).
  280.         if(inner==TRACKOUT) RCServo0.write(CENTER-35);
  281.         else RCServo0.write(CENTER+35);
  282.         tRun=millis();//brake;
  283.         curveFactor=0;
  284.         lanestage=1;//NEXT STAGE
  285.         laneon=1;//THIS IS TRUE, SINCE SENSORS ARE STILL ON THE ORIGINAL LANE
  286.       }else if(lanestage==1){//1. Detect once we have left the initial lane, then set angle straight
  287.         tRun=millis();//brake;
  288.         if(laneon){//STILL ON INITIAL LANE?
  289.           laneon=0;//ASSUME IT IS OFF THE LANE
  290.           for(int i=8;i<=15;i++){
  291.             if(digitalRead(i)){
  292.               laneon=1;//ONE SENSOR DETECTED SOMETHING, THEREFORE WE HAVEN\'T LEFT THE INITIAL LANE
  293.               lanelastsen=i;
  294.               break;//WE KNOW WE\'RE ON INITIAL LANE, NO POINT IN SCANNING OTHER SENSORS
  295.             }
  296.           }
  297.           //It\'s possible to go from 00100000->00000000->00010000
  298.           //We assume we have left the lane if we have 00000000 and the last state was 00000001 or 10000000
  299.           //However, we still might go from 00000001->00000000->00000001 (which happened before)
  300.           if(!(lanelastsen==8||lanelastsen==15)) laneon=1;//while we may have 00000000, the last detection wasn\'t on one endpoint, so we must still be on the lane.
  301.         }else{
  302.           delay(delaytime);//Just to be safe; might not need this
  303.           RCServo0.write(CENTER);//Go straight!
  304.           lanestage=2;//NEXT STAGE
  305.         }
  306.       }else if(lanestage==2){//Stage 2. Detect once we have reached a track. //NEW THEN TURN
  307.       //If we are going from OUTER to INNER, we should first detect something on one of the leftmost sensors (otherwise we failed).
  308.         if(inner==TRACKOUT){//Going from outer to inner
  309.           for(int i=8;i<=15;i++){
  310.             if(digitalRead(i)){
  311.               if(i<=11) RCServo0.write(CENTER+20);//Success! We detected a track on the left!
  312.               else inner=TRACKIN;//Failure! We are back on original track! Treat it as though we were initially on inner track and have gone to outer track.
  313.               lanestage=3;//NEXT STAGE
  314.               break;
  315.             }
  316.           }
  317.         }else{//same as above but opposite
  318.           for(int i=8;i<=15;i++){
  319.             if(digitalRead(i)){
  320.               if(i>=12) RCServo0.write(CENTER-20);
  321.               else inner=TRACKOUT;
  322.               lanestage=3;
  323.               break;
  324.             }
  325.           }
  326.         }  
  327.       }else if(lanestage==3){//Stage 3. Don\'t do anything until you see something on one of the four sensors on the side AWAY from the new track (Otherwise it won\'t turn properly).
  328.         if(inner==TRACKOUT){
  329.           for(int i=12;i<=15;i++){
  330.             if(digitalRead(i)) lanestage=4;
  331.           }
  332.         }else{
  333.           for(int i=8;i<=11;i++){        
  334.             if(digitalRead(i)) lanestage=4;
  335.           }
  336.         }
  337.       }else if(lanestage==4){//4. We did it! Well done
  338.         inner=!inner;//The state of the lane has changed.
  339.         lanestage=0;
  340.         lanechange=0;
  341.       }
  342.     }//END OF LANE CHANGE PROCESS
  343.    
  344.     //MOTOR SPEED CONTROL
  345.     if(millis()-tRun>ACCEL_CONSTANT){
  346.       analogWrite(5,motorSpeed);
  347.     }else if(millis()-tRun>BRAKE_CONSTANT){
  348.       analogWrite(5,1023);//MAX POWER!!!
  349.     }else{
  350.       analogWrite(5,min(motorSpeedCurve[curveFactor],motorSpeed));
  351.     }
  352.     //ACTUAL LANE CHANGE DETECTOR
  353.     if(lanet==0&&lanechange==0&&digitalRead(0)){
  354.       lanetime=millis();
  355.       lanet=1;
  356.     }else if(lanet==1){
  357.       if(!digitalRead(0)) lanet=0;
  358.       else if(millis()-lanetime>RFTHRESH){
  359.         lanechange=1;
  360.         lanet=2;
  361.       }
  362.     }else if(lanet==2){
  363.       if(!digitalRead(0)) lanet=0;
  364.       else if(millis()-lanetime>STOP_THRESH){
  365.         analogWrite(5,200);
  366.         delay(STOP_TIME);
  367.         analogWrite(5,1023);
  368.         delay(ACCEL_CONSTANT);
  369.         lanet=0;
  370.       }
  371.     }
  372.   }//END OF FAST LOOP
  373.  
  374.   //These slow things below are done oonly once per many times that the above fast stuff loops!
  375.   motorSpeed=knob(6);
  376.   if(stopbutton()){
  377.     iord=!iord;
  378.     LCD.clear();LCD.home();
  379.     if(iord){
  380.       LCD.clear();LCD.home();
  381.       LCD.print("KNOB 7: delaytime");
  382.       LCD.setCursor(0,1);
  383.       LCD.print("KNOB 6: POWER");
  384.       while(stopbutton()){delay(300);}
  385.     }else{
  386.       LCD.clear();LCD.home();
  387.       LCD.print("KNOB 7: KD");
  388.       LCD.setCursor(0,1);
  389.       LCD.print("KNOB 6: POWER");
  390.       while(stopbutton()){delay(300);}
  391.     }
  392.   }
  393.   if(startbutton()){
  394.     while(startbutton()){delay(100);}
  395.     if(iord){
  396.       while(!startbutton()){
  397.         LCD.clear();LCD.home();
  398.         delaytime = knob(7);
  399.         motorSpeed=knob(6);
  400.         LCD.print("DELAY:"); LCD.print(delaytime);
  401.         LCD.print(" PWR:"); LCD.print(motorSpeed);
  402.         LCD.setCursor(0,1);
  403.         for(int i=8;i<=15;i++){
  404.           if(digitalRead(i)) LCD.print("1");
  405.           else               LCD.print("0");
  406.         }
  407.         delay(200);
  408.       }
  409.     }else{
  410.       while(!startbutton()){
  411.         LCD.clear();LCD.home();
  412.         kd = knob(7)/10;
  413.         motorSpeed=knob(6);
  414.         LCD.print("KD:"); LCD.print(kd);
  415.         LCD.print(" PWR:"); LCD.print(motorSpeed);
  416.         LCD.setCursor(0,1);
  417.         for(int i=8;i<=15;i++){
  418.           if(digitalRead(i)) LCD.print("1");
  419.           else               LCD.print("0");
  420.         }
  421.        
  422.         delay(200);
  423.       }
  424.     }
  425.     while(startbutton()){delay(100);}
  426.   }  
  427.  
  428.   //END OF CODE
  429.   LCD.clear(); LCD.home();
  430.   LCD.print("rngfnd:");LCD.print(digitalRead(0)?"1":"0");
  431.   LCD.print(" pwr");LCD.print(motorSpeed);
  432.   //LCD.print(lanechange?("LANE CHANGE"):(" NO  CHANGE"));
  433.   LCD.setCursor(0,1);
  434.   LCD.print(inner?("INNER"):("OUTER"));
  435.   if(lanechange){
  436.     LCD.print("..SWITCHING");
  437.   }
  438. }
');