-
Notifications
You must be signed in to change notification settings - Fork 0
/
code.ino
265 lines (236 loc) · 6.64 KB
/
code.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
/////////Ball and Plate///////////////////////////////
/*
BALL AND PLATE PID CONTROL
*/
//////////////////////////////////////////////////////
///Libraries///
#include <PID_v1.h>
#include <stdint.h>
#include "TouchScreen.h"
#include <SPI.h>
#include <Wire.h>
#include <wiinunchuk.h>
#include<Servo.h>
// 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 ;
float xVal , yVal ;
int cCount=0;
int flagC=0;
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 = 151.0 / 955.0; // converts raw x values to mm. found through manual calibration
float convertY = 91.0 / 927.0; // converts raw y values to mm. found through manual calibration
/////TIME SAMPLE
int Ts = 50;
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, DIRECT);
PID myPID1(&Input1, &Output1, &Setpoint1,Kp1,Ki1,Kd1, DIRECT);
void setup()
{
servo1.attach(5);
servo2.attach(6);
Output=95;
Output1=95;
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(115200);
//INIT OF TOUSCHSCREEN
TSPoint p = ts.getPoint();
Input=120;
Input1=65;
//INIT SETPOINT
Setpoint=120;
Setpoint1=65;
//// 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);
myPID1.SetMode(AUTOMATIC);
myPID1.SetOutputLimits(20, 160);
// TIME SAMPLE
myPID1.SetSampleTime(Ts);
myPID.SetSampleTime(Ts);
/////
delay(100);
///
}
void loop()
{
while(Stable<125) //REGULATION LOOP
{
TSPoint p = ts.getPoint(); //measure pressure on plate
if (p.z > ts.pressureThreshhold) //ball is on plate
{
servo1.attach(5); //connect servos
servo2.attach(6);
setDesiredPosition();
noTouchCount = 0;
TSPoint p = ts.getPoint(); // measure actual position
Input=(p.x * convertX); // read and convert X coordinate
Input1=(p.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=95; //make plate flat
Output=93;
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();
///KONTROLA STABILITY////
while(Stable==125)//if is stable
{ //still measure actual postiion
setDesiredPosition();
TSPoint p = ts.getPoint();
Input=(p.x * convertX); //read X
Input1=(p.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;
}
if (nunchuk_cbutton()&&fl==1)///LEMNISCATE TRAJECOTRY
{
Setpoint = 85+ (50*cos(k))/(1+sin(k)*sin(k));
Setpoint1 = 55+ (50*sin(k)*cos(k))/(1+sin(k)*sin(k));
buttonPushCounter=0;
k=k+0.008;
}
if (nunchuk_cbutton()&&fl==2)// CIRCLE TRAJECTORY
{
Setpoint = 85+ 25*cos(k);
Setpoint1 = 55+ 25*sin(k);
buttonPushCounter=0;
k=k-0.02;
}
if (nunchuk_cbutton()&&fl==3)/// ELLIPSE TRAJECORY
{
Setpoint = 85+ 40*cos(k);
Setpoint1 = 55+ 25*sin(k);
buttonPushCounter=0;
k=k-0.02;
}
if (nunchuk_cbutton()&&fl==4) //PENTAGRAM TRAJECOTRY
{
Setpoint =85+ 18*cos(k)+12*cos(k*150);//
Setpoint1 =55+ 18*sin(k)-12*sin(k*150);//
buttonPushCounter=0;
k=k+0.01;
}
}