User | Revision | Line number | New contents of line |
gtg795y |
2:0a3c369d0076
|
1
|
/**
|
gtg795y |
2:0a3c369d0076
|
2
|
* Includes
|
gtg795y |
2:0a3c369d0076
|
3
|
*/
|
gtg795y |
1:d0f6e726b244
|
4
|
#include "Serializer.h"
|
gtg795y |
1:d0f6e726b244
|
5
|
|
gtg795y |
1:d0f6e726b244
|
6
|
|
gtg795y |
1:d0f6e726b244
|
7
|
Serializer::Serializer() {
|
gtg795y |
1:d0f6e726b244
|
8
|
serial=new Serial(p28,p27);
|
gtg795y |
1:d0f6e726b244
|
9
|
wait(2);
|
gtg795y |
1:d0f6e726b244
|
10
|
}
|
gtg795y |
1:d0f6e726b244
|
11
|
|
gtg795y |
1:d0f6e726b244
|
12
|
Serializer::~Serializer() {
|
gtg795y |
1:d0f6e726b244
|
13
|
delete serial;
|
gtg795y |
1:d0f6e726b244
|
14
|
}
|
gtg795y |
1:d0f6e726b244
|
15
|
|
gtg795y |
1:d0f6e726b244
|
16
|
void Serializer::Stop() {
|
gtg795y |
1:d0f6e726b244
|
17
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
18
|
serial->printf("stop\r");
|
gtg795y |
1:d0f6e726b244
|
19
|
}
|
gtg795y |
1:d0f6e726b244
|
20
|
|
gtg795y |
1:d0f6e726b244
|
21
|
void Serializer::ClearCountLeft() {
|
gtg795y |
1:d0f6e726b244
|
22
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
23
|
serial->printf("clrenc 1\r");
|
gtg795y |
1:d0f6e726b244
|
24
|
}
|
gtg795y |
1:d0f6e726b244
|
25
|
void Serializer::ClearCountRight() {
|
gtg795y |
1:d0f6e726b244
|
26
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
27
|
serial->printf("clrenc 2\r");
|
gtg795y |
1:d0f6e726b244
|
28
|
}
|
gtg795y |
1:d0f6e726b244
|
29
|
|
gtg795y |
1:d0f6e726b244
|
30
|
void Serializer::ClearCount() {
|
gtg795y |
1:d0f6e726b244
|
31
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
32
|
serial->printf("clrenc 1 2\r");
|
gtg795y |
1:d0f6e726b244
|
33
|
}
|
gtg795y |
1:d0f6e726b244
|
34
|
|
gtg795y |
1:d0f6e726b244
|
35
|
void Serializer::SetSpeedLeft(int speed) {
|
gtg795y |
1:d0f6e726b244
|
36
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
37
|
serial->printf("mogo 1:%i\r", -speed);
|
gtg795y |
1:d0f6e726b244
|
38
|
}
|
gtg795y |
1:d0f6e726b244
|
39
|
void Serializer::SetSpeedRight(int speed) {
|
gtg795y |
1:d0f6e726b244
|
40
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
41
|
serial->printf("mogo 2:%i\r", -speed);
|
gtg795y |
1:d0f6e726b244
|
42
|
}
|
gtg795y |
1:d0f6e726b244
|
43
|
void Serializer::SetSpeed(int speed) {
|
gtg795y |
1:d0f6e726b244
|
44
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
45
|
serial->printf("mogo 1:%i 2:%i\r", -speed, -speed);
|
gtg795y |
1:d0f6e726b244
|
46
|
}
|
gtg795y |
1:d0f6e726b244
|
47
|
|
gtg795y |
1:d0f6e726b244
|
48
|
void Serializer::SetVPID(int p,int i,int d,int l) {
|
gtg795y |
1:d0f6e726b244
|
49
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
50
|
serial->printf("vpid %i:%i:%i:%i\r", p,i,d,l);
|
gtg795y |
1:d0f6e726b244
|
51
|
}
|
gtg795y |
1:d0f6e726b244
|
52
|
|
gtg795y |
1:d0f6e726b244
|
53
|
void Serializer::SetDPID(int p,int i,int d,int a) {
|
gtg795y |
1:d0f6e726b244
|
54
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
55
|
serial->printf("dpid %i:%i:%i:%i\r", p,i,d,a);
|
gtg795y |
1:d0f6e726b244
|
56
|
}
|
gtg795y |
1:d0f6e726b244
|
57
|
|
gtg795y |
1:d0f6e726b244
|
58
|
void Serializer::DiGoLeft(int dist,int speed) {
|
gtg795y |
1:d0f6e726b244
|
59
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
60
|
serial->printf("digo 1:%i:%i\r",-dist, speed);
|
gtg795y |
1:d0f6e726b244
|
61
|
}
|
gtg795y |
1:d0f6e726b244
|
62
|
void Serializer::DiGoRight(int dist,int speed) {
|
gtg795y |
1:d0f6e726b244
|
63
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
64
|
serial->printf("digo 2:%i:%i\r",-dist, speed);
|
gtg795y |
1:d0f6e726b244
|
65
|
}
|
gtg795y |
1:d0f6e726b244
|
66
|
void Serializer::DiGo(int dist,int speed) {
|
gtg795y |
1:d0f6e726b244
|
67
|
if (serial->writeable())
|
gtg795y |
1:d0f6e726b244
|
68
|
serial->printf("digo 1:%i:%i 2:%i:%i\r",-dist, speed, -dist, speed);
|
gtg795y |
1:d0f6e726b244
|
69
|
}
|
gtg795y |
1:d0f6e726b244
|
70
|
|
gtg795y |
1:d0f6e726b244
|
71
|
|
gtg795y |
1:d0f6e726b244
|
72
|
|
gtg795y |
1:d0f6e726b244
|
73
|
//unused and untested
|
gtg795y |
1:d0f6e726b244
|
74
|
int Serializer::GetReply() {
|
gtg795y |
1:d0f6e726b244
|
75
|
int returnValue=0;
|
gtg795y |
1:d0f6e726b244
|
76
|
char c=0;
|
gtg795y |
1:d0f6e726b244
|
77
|
char readyToReturn=0;
|
gtg795y |
1:d0f6e726b244
|
78
|
char ack=0;
|
gtg795y |
1:d0f6e726b244
|
79
|
char nack=0;
|
gtg795y |
1:d0f6e726b244
|
80
|
wait(0.0001);
|
gtg795y |
1:d0f6e726b244
|
81
|
while (serial->readable()) {
|
gtg795y |
1:d0f6e726b244
|
82
|
c=serial->getc();
|
gtg795y |
1:d0f6e726b244
|
83
|
if(readyToReturn)continue;
|
gtg795y |
1:d0f6e726b244
|
84
|
if (c=='N'&&nack==0)nack=1;
|
gtg795y |
1:d0f6e726b244
|
85
|
else nack=0;
|
gtg795y |
1:d0f6e726b244
|
86
|
if (c=='A'&&ack==0)ack++;
|
gtg795y |
1:d0f6e726b244
|
87
|
else ack=nack=0;
|
gtg795y |
1:d0f6e726b244
|
88
|
if (c=='C'&&ack==1)ack++;
|
gtg795y |
1:d0f6e726b244
|
89
|
else ack=nack=0;
|
gtg795y |
1:d0f6e726b244
|
90
|
if (c=='K'&&ack==2)ack++;
|
gtg795y |
1:d0f6e726b244
|
91
|
else ack=nack=0;
|
gtg795y |
1:d0f6e726b244
|
92
|
if (ack==3)
|
gtg795y |
1:d0f6e726b244
|
93
|
if (nack==1)
|
gtg795y |
1:d0f6e726b244
|
94
|
returnValue=1;
|
gtg795y |
1:d0f6e726b244
|
95
|
else
|
gtg795y |
1:d0f6e726b244
|
96
|
returnValue=0;
|
gtg795y |
1:d0f6e726b244
|
97
|
if(c=='>')readyToReturn=1;
|
gtg795y |
1:d0f6e726b244
|
98
|
}
|
gtg795y |
1:d0f6e726b244
|
99
|
return returnValue;
|
gtg795y |
1:d0f6e726b244
|
100
|
}
|