Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- ///Libraries///
- #include <PID_v1.h>
- #include <stdint.h>
- // #include "TouchScreen.h"
- //#include <SPI.h>
- #include <Wire.h>
- //#include <wiinunchuk.h>
- #include <Servo.h>
- #define MODE 0
- // Definitions TOUCH PINS
- /* #define YP A0 //0
- #define XM A1 //1
- #define YM 3 //3
- #define XP 4 //4
- TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);
- int buttonPushCounter = 1; // counter for the number of button presses
- int lastButtonState = 0; // previous state of the button */
- // int flag, flagZ;
- bool touch;
- int noTouchCount=0;
- float x, y;
- void getxy()
- {
- int serial[18];
- for (int i = 0; i < 9; i++)
- {
- int num = Serial.read();
- serial[i] = num;
- }
- for (int i = 9; i < 18; i++)
- {
- serial[i] = serial[i - 9];
- }
- for (int i = 0; i < 18; i++)
- {
- if (i > 5)
- if (serial[i] == 255)
- if (serial[i + 1] == 255)
- if (serial[i + 2] == 255)
- {
- x = serial[i - 5] * 255 + serial[i - 4];
- y = serial[i - 3] * 255 + serial[i - 2];
- touch = serial[i - 1];
- }
- }
- }
- // int cCount = 0;
- // int \ int flagK = 0;
- // float kk = 0;
- // int fl = 0;
- // double l = 0.00;
- // unsigned int noTouchCount = 0; //viariable for noTouch
- double k = 0;
- // PID values
- double Setpoint, Input, Output; //for X
- double Setpoint1, Input1, Output1; //for Y
- // int Modulo;
- // long lastcas = 0;
- // servos variables
- Servo servo1; //X axis
- Servo servo2; //Y axis
- /*
- uint16_t homeX = 550; // raw data value for center of touchscreen
- uint16_t homeY = 550; // raw data value for center of touchscreen */
- float convertX = 154.08 / 790.0; // converts raw x values to mm. found through manual calibration
- float convertY = 85.92 / 470.0; // converts raw y values to mm. found through manual calibration
- /////TIME SAMPLE
- int Ts = 0;
- unsigned long Stable = 0;
- //PID const
- float Kp = 0.3;
- float Ki = 0.03;
- float Kd = 0.13;
- float Kp1 = 0.3;
- float Ki1 = 0.08;
- float Kd1 = 0.13;
- // long cas = 0;
- //INIT PID
- PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, REVER);
- PID myPID1(&Input1, &Output1, &Setpoint1, Kp1, Ki1, Kd1, DIRECT);
- void setup()
- {
- pinMode(0, INPUT);
- servo1.attach(5);
- servo2.attach(6);
- Output = 60;
- Output1 = 4;
- servo1.write(Output);
- servo2.write(Output1);
- /* //init NUN
- nunchuk_setpowerpins();
- nunchuk_init();
- nunchuk_get_data(); */
- //INIT PINS
- /* pinMode(9, OUTPUT);
- pinMode(8, OUTPUT);
- digitalWrite(9, LOW); //LED INIT
- digitalWrite(8, LOW); */
- Serial.begin(9600);
- //INIT OF TOUSCHSCREEN
- getxy();
- Input = 120;//10
- Input1 = 65;//3
- //INIT SETPOINT
- Setpoint = 120;//7.7
- Setpoint1 = 65;//4.3
- //// Make plate flat
- /* servo1.attach(5);
- servo2.attach(6);
- Output = 95;
- Output1 = 95;
- servo1.write(Output);
- servo2.write(Output1); */
- //Zapnutie PID
- myPID.SetMode(AUTOMATIC);
- myPID.SetOutputLimits(20, 160);//10.140
- myPID1.SetMode(AUTOMATIC);
- myPID1.SetOutputLimits(20, 160);//10,76
- // TIME SAMPLE
- myPID1.SetSampleTime(Ts);
- myPID.SetSampleTime(Ts);
- delay(100);
- }
- void loop()
- {
- while (Stable < 125) //REGULATION LOOP
- {
- int oldx = x, oldy = y;
- getxy();
- if ((x != oldx) || (y != oldy)) //ball is on plate
- {
- servo1.attach(5); //connect servos
- servo2.attach(6);
- setDesiredPosition();
- noTouchCount = 0;
- getxy(); // measure actual position
- Input = (x * convertX); // read and convert X coordinate
- Input1 = (y * convertY); // read and convert Y coordinate
- if ((Input > Setpoint - 2 && Input < Setpoint + 2 && Input1 > Setpoint1 - 2 && Input1 < Setpoint1 + 2)) //if ball is close to setpoint
- {
- Stable = Stable + 1; //increment STABLE
- // digitalWrite(9, HIGH);
- }
- /* else
- {
- digitalWrite(9, LOW);
- } */
- myPID.Compute(); //action control X compute
- myPID1.Compute(); // action control Y compute
- }
- else //if there is no ball on plate
- {
- noTouchCount++; //increment no touch count
- if (noTouchCount == 75)
- {
- noTouchCount++;
- Output = 60; //make plate flat
- Output1 = 4;
- servo1.write(Output);
- servo2.write(Output1);
- }
- /* if (noTouchCount == 150) //if there is no ball on plate longer
- {
- servo1.detach(); //detach servos
- servo2.detach();
- } */
- }
- servo1.write(Output); //control
- servo2.write(Output1); //control
- Serial.print(Setpoint);
- Serial.print(",");
- Serial.print(Setpoint1);
- Serial.print(",");
- Serial.print(Input);
- Serial.print(",");
- Serial.println(Input1);
- } ////END OF REGULATION LOOP///
- /* servo1.detach(); //detach servos
- servo2.detach(); */
- ///control STABILITY////
- while (Stable == 125) //if is stable
- { //still measure actual postiion
- setDesiredPosition();
- getxy(); //alternative
- Input = (x * convertX); //read X
- Input1 = (y * convertY); //read Y
- if (Input < Setpoint - 2 || Input > Setpoint + 2 || Input1 > Setpoint1 + 2 || Input1 < Setpoint1 - 2) //if ball isnt close to setpoint
- {
- servo1.attach(5); //again attach servos
- servo2.attach(6);
- // digitalWrite(9, LOW);
- Stable = 0; //change STABLE state
- }
- } //end of STABLE LOOP
- } //loop end
- ////////////////////////Functions//////////////////
- ///// DESIRED POSITION
- void setDesiredPosition()
- {
- // nunchuk_get_data();
- //if zbutton is pressed, zero positions
- /* int c = nunchuk_zbutton();
- if (c != lastButtonState)
- {
- // if the state has changed, increment the counter
- if (c == HIGH && digitalRead(11) == 0)
- {
- // if the current state is HIGH then the button
- // wend from off to on:
- buttonPushCounter++;
- }
- }
- lastButtonState = c; */
- /* switch (buttonPushCounter)
- {
- case 1:
- Setpoint = 120;
- Setpoint1 = 70;
- fl = 1;
- break;
- case 2:
- Setpoint = 52;
- Setpoint1 = 70;
- fl = 2;
- break;
- case 3:
- Setpoint = 52;
- Setpoint1 = 40;
- fl = 3;
- break;
- case 4:
- Setpoint = 120;
- Setpoint1 = 40;
- buttonPushCounter = 0;
- fl = 4;
- break;
- } */
- switch (MODE)
- {
- case 0:
- Setpoint = 120;//
- Setpoint1 = 70;//
- break;
- case 1:
- Setpoint = 85 + (50 * cos(k)) / (1 + sin(k) * sin(k));
- Setpoint1 = 55 + (50 * sin(k) * cos(k)) / (1 + sin(k) * sin(k));
- k = k + 0.008;
- break;
- case 2:
- Setpoint = 85 + 25 * cos(k);
- Setpoint1 = 55 + 25 * sin(k);
- k = k - 0.02;
- break;
- case 3:
- Setpoint = 85 + 40 * cos(k);
- Setpoint1 = 55 + 25 * sin(k);
- k = k - 0.02;
- break;
- case 4:
- Setpoint = 85 + 18 * cos(k) + 12 * cos(k * 150); //
- Setpoint1 = 55 + 18 * sin(k) - 12 * sin(k * 150); //
- k = k + 0.01;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement