SHARE
TWEET

Untitled

a guest Mar 21st, 2019 74 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. ///Libraries///
  2. #include <PID_v1.h>
  3. #include <stdint.h>
  4. // #include "TouchScreen.h"
  5. //#include <SPI.h>
  6. #include <Wire.h>
  7. //#include <wiinunchuk.h>
  8. #include <Servo.h>
  9.  
  10. #define MODE 0
  11.  
  12. // Definitions TOUCH PINS
  13. /* #define YP A0 //0
  14.   #define XM A1 //1
  15.   #define YM 3  //3
  16.   #define XP 4  //4
  17.   TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);
  18.   int buttonPushCounter = 1; // counter for the number of button presses
  19.   int lastButtonState = 0;   // previous state of the button */
  20. // int flag, flagZ;
  21. bool touch;
  22. int noTouchCount=0;
  23. float x, y;
  24. void getxy()
  25. {
  26.   int serial[18];
  27.   for (int i = 0; i < 9; i++)
  28.   {
  29.     int num = Serial.read();
  30.     serial[i] = num;
  31.   }
  32.   for (int i = 9; i < 18; i++)
  33.   {
  34.     serial[i] = serial[i - 9];
  35.   }
  36.   for (int i = 0; i < 18; i++)
  37.   {
  38.     if (i > 5)
  39.       if (serial[i] == 255)
  40.         if (serial[i + 1] == 255)
  41.           if (serial[i + 2] == 255)
  42.           {
  43.             x = serial[i - 5] * 255 + serial[i - 4];
  44.             y = serial[i - 3] * 255 + serial[i - 2];
  45.             touch = serial[i - 1];
  46.           }
  47.  
  48.   }
  49. }
  50. // int cCount = 0;
  51. // int \ int flagK = 0;
  52. // float kk = 0;
  53. // int fl = 0;
  54. // double l = 0.00;
  55. // unsigned int noTouchCount = 0; //viariable for noTouch
  56. double k = 0;
  57.  
  58. // PID values
  59. double Setpoint, Input, Output;    //for X
  60. double Setpoint1, Input1, Output1; //for Y
  61.  
  62. // int Modulo;
  63. // long lastcas = 0;
  64.  
  65. // servos variables
  66. Servo servo1; //X axis
  67. Servo servo2; //Y axis
  68. /*
  69.   uint16_t homeX = 550; // raw data value for center of touchscreen
  70.   uint16_t homeY = 550; // raw data value for center of touchscreen */
  71.  
  72. float convertX = 154.08 / 790.0; // converts raw x values to mm. found through manual calibration
  73. float convertY = 85.92 / 470.0;  // converts raw y values to mm. found through manual calibration
  74.  
  75. /////TIME SAMPLE
  76. int Ts = 0;
  77. unsigned long Stable = 0;
  78.  
  79. //PID const
  80. float Kp = 0.3;
  81. float Ki = 0.03;
  82. float Kd = 0.13;
  83.  
  84. float Kp1 = 0.3;
  85. float Ki1 = 0.08;
  86. float Kd1 = 0.13;
  87. // long cas = 0;
  88.  
  89. //INIT PID
  90. PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, REVER);
  91. PID myPID1(&Input1, &Output1, &Setpoint1, Kp1, Ki1, Kd1, DIRECT);
  92.  
  93. void setup()
  94. {
  95.   pinMode(0, INPUT);
  96.   servo1.attach(5);
  97.   servo2.attach(6);
  98.   Output = 60;
  99.   Output1 = 4;
  100.   servo1.write(Output);
  101.   servo2.write(Output1);
  102.  
  103.   /*   //init NUN
  104.     nunchuk_setpowerpins();
  105.     nunchuk_init();
  106.     nunchuk_get_data(); */
  107.  
  108.   //INIT PINS
  109.   /*     pinMode(9, OUTPUT);
  110.     pinMode(8, OUTPUT);
  111.     digitalWrite(9, LOW); //LED INIT
  112.     digitalWrite(8, LOW); */
  113.  
  114.   Serial.begin(9600);
  115.  
  116.   //INIT OF TOUSCHSCREEN
  117.   getxy();
  118.   Input = 120;//10
  119.   Input1 = 65;//3
  120.   //INIT SETPOINT
  121.   Setpoint = 120;//7.7
  122.   Setpoint1 = 65;//4.3
  123.   //// Make plate flat
  124.   /*  servo1.attach(5);
  125.     servo2.attach(6);
  126.     Output = 95;
  127.     Output1 = 95;
  128.     servo1.write(Output);
  129.     servo2.write(Output1); */
  130.  
  131.   //Zapnutie PID
  132.   myPID.SetMode(AUTOMATIC);
  133.   myPID.SetOutputLimits(20, 160);//10.140
  134.   myPID1.SetMode(AUTOMATIC);
  135.   myPID1.SetOutputLimits(20, 160);//10,76
  136.   // TIME SAMPLE
  137.   myPID1.SetSampleTime(Ts);
  138.   myPID.SetSampleTime(Ts);
  139.  
  140.   delay(100);
  141. }
  142.  
  143. void loop()
  144. {
  145.   while (Stable < 125) //REGULATION LOOP
  146.   {
  147.     int oldx = x, oldy = y;
  148.     getxy();
  149.     if ((x != oldx) || (y != oldy)) //ball is on plate
  150.     {
  151.       servo1.attach(5); //connect servos
  152.       servo2.attach(6);
  153.       setDesiredPosition();
  154.       noTouchCount = 0;
  155.       getxy(); // measure actual position
  156.       Input = (x * convertX);    // read and convert X coordinate
  157.       Input1 = (y * convertY);   // read and convert Y coordinate
  158.  
  159.       if ((Input > Setpoint - 2 && Input < Setpoint + 2 && Input1 > Setpoint1 - 2 && Input1 < Setpoint1 + 2)) //if ball is close to setpoint
  160.       {
  161.         Stable = Stable + 1; //increment STABLE
  162.         // digitalWrite(9, HIGH);
  163.       }
  164.       /* else
  165.         {
  166.           digitalWrite(9, LOW);
  167.         } */
  168.       myPID.Compute();  //action control X compute
  169.       myPID1.Compute(); //   action control  Y compute
  170.     }
  171.     else //if there is no ball on plate
  172.     {
  173.       noTouchCount++; //increment no touch count
  174.  
  175.       if (noTouchCount == 75)
  176.       {
  177.         noTouchCount++;
  178.         Output = 60; //make plate flat
  179.         Output1 = 4;
  180.         servo1.write(Output);
  181.         servo2.write(Output1);
  182.       }
  183.       /* if (noTouchCount == 150) //if there is no ball on plate longer
  184.         {
  185.           servo1.detach(); //detach servos
  186.           servo2.detach();
  187.         } */
  188.     }
  189.     servo1.write(Output);  //control
  190.     servo2.write(Output1); //control
  191.     Serial.print(Setpoint);
  192.     Serial.print(",");
  193.     Serial.print(Setpoint1);
  194.     Serial.print(",");
  195.     Serial.print(Input);
  196.     Serial.print(",");
  197.     Serial.println(Input1);
  198.  
  199.   } ////END OF REGULATION LOOP///
  200.  
  201. /*   servo1.detach(); //detach servos
  202.   servo2.detach(); */
  203.  
  204.   ///control STABILITY////
  205.   while (Stable == 125) //if is stable
  206.   { //still measure actual postiion
  207.     setDesiredPosition();
  208.     getxy();                                                                            //alternative
  209.     Input = (x * convertX);                                                                               //read X
  210.     Input1 = (y * convertY);                                                                              //read Y
  211.     if (Input < Setpoint - 2 || Input > Setpoint + 2 || Input1 > Setpoint1 + 2 || Input1 < Setpoint1 - 2) //if ball isnt close to setpoint
  212.     {
  213.       servo1.attach(5); //again attach servos
  214.       servo2.attach(6);
  215.       // digitalWrite(9, LOW);
  216.       Stable = 0; //change STABLE state
  217.     }
  218.  
  219.   } //end of STABLE LOOP
  220. } //loop end
  221.  
  222. ////////////////////////Functions//////////////////
  223. ///// DESIRED POSITION
  224. void setDesiredPosition()
  225. {
  226.  
  227.   // nunchuk_get_data();
  228.   //if zbutton is pressed, zero positions
  229.  
  230.   /* int c = nunchuk_zbutton();
  231.     if (c != lastButtonState)
  232.     {
  233.       // if the state has changed, increment the counter
  234.       if (c == HIGH && digitalRead(11) == 0)
  235.       {
  236.           // if the current state is HIGH then the button
  237.           // wend from off to on:
  238.           buttonPushCounter++;
  239.       }
  240.     }
  241.     lastButtonState = c; */
  242.  
  243.   /* switch (buttonPushCounter)
  244.     {
  245.     case 1:
  246.       Setpoint = 120;
  247.       Setpoint1 = 70;
  248.       fl = 1;
  249.       break;
  250.     case 2:
  251.       Setpoint = 52;
  252.       Setpoint1 = 70;
  253.       fl = 2;
  254.       break;
  255.     case 3:
  256.       Setpoint = 52;
  257.       Setpoint1 = 40;
  258.       fl = 3;
  259.       break;
  260.     case 4:
  261.       Setpoint = 120;
  262.       Setpoint1 = 40;
  263.       buttonPushCounter = 0;
  264.       fl = 4;
  265.       break;
  266.     } */
  267.  
  268.   switch (MODE)
  269.   {
  270.     case 0:
  271.       Setpoint = 120;//
  272.       Setpoint1 = 70;//
  273.       break;
  274.     case 1:
  275.       Setpoint = 85 + (50 * cos(k)) / (1 + sin(k) * sin(k));
  276.       Setpoint1 = 55 + (50 * sin(k) * cos(k)) / (1 + sin(k) * sin(k));
  277.       k = k + 0.008;
  278.       break;
  279.     case 2:
  280.       Setpoint = 85 + 25 * cos(k);
  281.       Setpoint1 = 55 + 25 * sin(k);
  282.       k = k - 0.02;
  283.       break;
  284.     case 3:
  285.       Setpoint = 85 + 40 * cos(k);
  286.       Setpoint1 = 55 + 25 * sin(k);
  287.       k = k - 0.02;
  288.       break;
  289.     case 4:
  290.       Setpoint = 85 + 18 * cos(k) + 12 * cos(k * 150);  //
  291.       Setpoint1 = 55 + 18 * sin(k) - 12 * sin(k * 150); //
  292.       k = k + 0.01;
  293.   }
  294. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top