User | Revision | Line number | New contents of line |
nucho |
0:3c49891bc39d
|
1
|
#include "mbed.h"
|
nucho |
0:3c49891bc39d
|
2
|
#include "RTno.h"
|
nucho |
0:3c49891bc39d
|
3
|
|
nucho |
0:3c49891bc39d
|
4
|
#include "QEI.h"
|
nucho |
0:3c49891bc39d
|
5
|
#include "SimplePID.h"
|
nucho |
0:3c49891bc39d
|
6
|
#define MOTOR_OFFSET 1460
|
nucho |
0:3c49891bc39d
|
7
|
#define KP 5.1
|
nucho |
0:3c49891bc39d
|
8
|
#define KI 0.0
|
nucho |
0:3c49891bc39d
|
9
|
#define KD 0.0
|
nucho |
0:3c49891bc39d
|
10
|
#define RATE 0.2
|
nucho |
0:3c49891bc39d
|
11
|
|
nucho |
0:3c49891bc39d
|
12
|
PwmOut motor1(p21);
|
nucho |
0:3c49891bc39d
|
13
|
QEI qei_motor1(p29, p30, NC, 624);
|
nucho |
0:3c49891bc39d
|
14
|
SimplePID pid_motor1(KP,KI,KD,RATE);
|
nucho |
0:3c49891bc39d
|
15
|
|
nucho |
0:3c49891bc39d
|
16
|
PwmOut motor2(p22);
|
nucho |
0:3c49891bc39d
|
17
|
QEI qei_motor2(p27, p28, NC, 624);
|
nucho |
0:3c49891bc39d
|
18
|
SimplePID pid_motor2(KP,KI,KD,RATE);
|
nucho |
0:3c49891bc39d
|
19
|
/**
|
nucho |
0:3c49891bc39d
|
20
|
* digitalInOut.pde
|
nucho |
0:3c49891bc39d
|
21
|
* RTno is RT-middleware and arduino.
|
nucho |
0:3c49891bc39d
|
22
|
*
|
nucho |
0:3c49891bc39d
|
23
|
* Using RTno, arduino device can communicate any RT-components
|
nucho |
0:3c49891bc39d
|
24
|
* through the RTno-proxy component which is launched in PC.
|
nucho |
0:3c49891bc39d
|
25
|
* Connect arduino with USB, and program with RTno library.
|
nucho |
0:3c49891bc39d
|
26
|
* You do not have to define any protocols to establish communication
|
nucho |
0:3c49891bc39d
|
27
|
* between arduino and PC.
|
nucho |
0:3c49891bc39d
|
28
|
*
|
nucho |
0:3c49891bc39d
|
29
|
* Using RTno, you must not define the function "setup" and "loop".
|
nucho |
0:3c49891bc39d
|
30
|
* Those functions are automatically defined in the RTno libarary.
|
nucho |
0:3c49891bc39d
|
31
|
* You, developers, must define following functions:
|
nucho |
0:3c49891bc39d
|
32
|
* int onInitialize(void);
|
nucho |
0:3c49891bc39d
|
33
|
* int onActivated(void);
|
nucho |
0:3c49891bc39d
|
34
|
* int onDeactivated(void);
|
nucho |
0:3c49891bc39d
|
35
|
* int onExecute(void);
|
nucho |
0:3c49891bc39d
|
36
|
* int onError(void);
|
nucho |
0:3c49891bc39d
|
37
|
* int onReset(void);
|
nucho |
0:3c49891bc39d
|
38
|
* These functions are spontaneously called by the RTno-proxy
|
nucho |
0:3c49891bc39d
|
39
|
* RT-component which is launched in the PC.
|
nucho |
0:3c49891bc39d
|
40
|
*/
|
nucho |
0:3c49891bc39d
|
41
|
|
nucho |
0:3c49891bc39d
|
42
|
|
nucho |
0:3c49891bc39d
|
43
|
/**
|
nucho |
0:3c49891bc39d
|
44
|
* This function is called at first.
|
nucho |
0:3c49891bc39d
|
45
|
* conf._default.baudrate: baudrate of serial communication
|
nucho |
0:3c49891bc39d
|
46
|
* exec_cxt.periodic.type: reserved but not used.
|
nucho |
0:3c49891bc39d
|
47
|
*/
|
nucho |
0:3c49891bc39d
|
48
|
void rtcconf(void) {
|
nucho |
0:3c49891bc39d
|
49
|
conf._default.baudrate = 115200;
|
nucho |
0:3c49891bc39d
|
50
|
exec_cxt.periodic.type = ProxySynchronousExecutionContext;
|
nucho |
0:3c49891bc39d
|
51
|
}
|
nucho |
0:3c49891bc39d
|
52
|
|
nucho |
0:3c49891bc39d
|
53
|
/**
|
nucho |
0:3c49891bc39d
|
54
|
* Declaration Division:
|
nucho |
0:3c49891bc39d
|
55
|
*
|
nucho |
0:3c49891bc39d
|
56
|
* DataPort and Data Buffer should be placed here.
|
nucho |
0:3c49891bc39d
|
57
|
*
|
nucho |
0:3c49891bc39d
|
58
|
* Currently, following 6 types are available.
|
nucho |
0:3c49891bc39d
|
59
|
* TimedLong:
|
nucho |
0:3c49891bc39d
|
60
|
* TimedDouble:
|
nucho |
0:3c49891bc39d
|
61
|
* TimedFloat:
|
nucho |
0:3c49891bc39d
|
62
|
* TimedLongSeq:
|
nucho |
0:3c49891bc39d
|
63
|
* TimedDoubleSeq:
|
nucho |
0:3c49891bc39d
|
64
|
* TimedFloatSeq:
|
nucho |
0:3c49891bc39d
|
65
|
*
|
nucho |
0:3c49891bc39d
|
66
|
* Please refer following comments. If you need to use some ports,
|
nucho |
0:3c49891bc39d
|
67
|
* uncomment the line you want to declare.
|
nucho |
0:3c49891bc39d
|
68
|
**/
|
nucho |
0:3c49891bc39d
|
69
|
TimedLongSeq position;
|
nucho |
0:3c49891bc39d
|
70
|
InPort positionIn("position", position);
|
nucho |
0:3c49891bc39d
|
71
|
|
nucho |
0:3c49891bc39d
|
72
|
TimedLongSeq encorder;
|
nucho |
0:3c49891bc39d
|
73
|
OutPort encorderOut("encorder", encorder);
|
nucho |
0:3c49891bc39d
|
74
|
|
nucho |
0:3c49891bc39d
|
75
|
|
nucho |
0:3c49891bc39d
|
76
|
//////////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
77
|
// on_initialize
|
nucho |
0:3c49891bc39d
|
78
|
//
|
nucho |
0:3c49891bc39d
|
79
|
// This function is called in the initialization
|
nucho |
0:3c49891bc39d
|
80
|
// sequence. The sequence is triggered by the
|
nucho |
0:3c49891bc39d
|
81
|
// PC. When the RTnoRTC is launched in the PC,
|
nucho |
0:3c49891bc39d
|
82
|
// then, this function is remotely called
|
nucho |
0:3c49891bc39d
|
83
|
// through the USB cable.
|
nucho |
0:3c49891bc39d
|
84
|
// In on_initialize, usually DataPorts are added.
|
nucho |
0:3c49891bc39d
|
85
|
//
|
nucho |
0:3c49891bc39d
|
86
|
//////////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
87
|
int RTno::onInitialize() {
|
nucho |
0:3c49891bc39d
|
88
|
/* Data Ports are added in this section.
|
nucho |
0:3c49891bc39d
|
89
|
*/
|
nucho |
0:3c49891bc39d
|
90
|
addInPort(positionIn);
|
nucho |
0:3c49891bc39d
|
91
|
addOutPort(encorderOut);
|
nucho |
0:3c49891bc39d
|
92
|
|
nucho |
0:3c49891bc39d
|
93
|
// Some initialization (like port direction setting)
|
nucho |
0:3c49891bc39d
|
94
|
|
nucho |
0:3c49891bc39d
|
95
|
return RTC_OK;
|
nucho |
0:3c49891bc39d
|
96
|
}
|
nucho |
0:3c49891bc39d
|
97
|
|
nucho |
0:3c49891bc39d
|
98
|
////////////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
99
|
// on_activated
|
nucho |
0:3c49891bc39d
|
100
|
// This function is called when the RTnoRTC
|
nucho |
0:3c49891bc39d
|
101
|
// is activated. When the activation, the RTnoRTC
|
nucho |
0:3c49891bc39d
|
102
|
// sends message to call this function remotely.
|
nucho |
0:3c49891bc39d
|
103
|
// If this function is failed (return value
|
nucho |
0:3c49891bc39d
|
104
|
// is RTC_ERROR), RTno will enter ERROR condition.
|
nucho |
0:3c49891bc39d
|
105
|
////////////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
106
|
int RTno::onActivated() {
|
nucho |
0:3c49891bc39d
|
107
|
// Write here initialization code.
|
nucho |
0:3c49891bc39d
|
108
|
|
nucho |
0:3c49891bc39d
|
109
|
return RTC_OK;
|
nucho |
0:3c49891bc39d
|
110
|
}
|
nucho |
0:3c49891bc39d
|
111
|
|
nucho |
0:3c49891bc39d
|
112
|
/////////////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
113
|
// on_deactivated
|
nucho |
0:3c49891bc39d
|
114
|
// This function is called when the RTnoRTC
|
nucho |
0:3c49891bc39d
|
115
|
// is deactivated.
|
nucho |
0:3c49891bc39d
|
116
|
/////////////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
117
|
int RTno::onDeactivated() {
|
nucho |
0:3c49891bc39d
|
118
|
// Write here finalization code.
|
nucho |
0:3c49891bc39d
|
119
|
|
nucho |
0:3c49891bc39d
|
120
|
return RTC_OK;
|
nucho |
0:3c49891bc39d
|
121
|
}
|
nucho |
0:3c49891bc39d
|
122
|
|
nucho |
0:3c49891bc39d
|
123
|
//////////////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
124
|
// This function is repeatedly called when the
|
nucho |
0:3c49891bc39d
|
125
|
// RTno is in the ACTIVE condition.
|
nucho |
0:3c49891bc39d
|
126
|
// If this function is failed (return value is
|
nucho |
0:3c49891bc39d
|
127
|
// RTC_ERROR), RTno immediately enter into the
|
nucho |
0:3c49891bc39d
|
128
|
// ERROR condition.r
|
nucho |
0:3c49891bc39d
|
129
|
//////////////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
130
|
int RTno::onExecute() {
|
nucho |
0:3c49891bc39d
|
131
|
|
nucho |
0:3c49891bc39d
|
132
|
/*
|
nucho |
0:3c49891bc39d
|
133
|
* Input
|
nucho |
0:3c49891bc39d
|
134
|
*/
|
nucho |
0:3c49891bc39d
|
135
|
if (positionIn.isNew()) {
|
nucho |
0:3c49891bc39d
|
136
|
positionIn.read();
|
nucho |
0:3c49891bc39d
|
137
|
pid_motor1.setGoal(position.data[0]);
|
nucho |
0:3c49891bc39d
|
138
|
pid_motor2.setGoal(position.data[1]);
|
nucho |
0:3c49891bc39d
|
139
|
}
|
nucho |
0:3c49891bc39d
|
140
|
|
nucho |
0:3c49891bc39d
|
141
|
pid_motor1.setLimits(-500,500);
|
nucho |
0:3c49891bc39d
|
142
|
pid_motor2.setLimits(-500,500);
|
nucho |
0:3c49891bc39d
|
143
|
|
nucho |
0:3c49891bc39d
|
144
|
/*
|
nucho |
0:3c49891bc39d
|
145
|
* Output
|
nucho |
0:3c49891bc39d
|
146
|
*/
|
nucho |
0:3c49891bc39d
|
147
|
int current1,current2;
|
nucho |
0:3c49891bc39d
|
148
|
current1 = qei_motor1.getPulses();
|
nucho |
0:3c49891bc39d
|
149
|
current2 = qei_motor2.getPulses();
|
nucho |
0:3c49891bc39d
|
150
|
|
nucho |
0:3c49891bc39d
|
151
|
encorder.data.length(2);
|
nucho |
0:3c49891bc39d
|
152
|
encorder.data[0] = current1;
|
nucho |
0:3c49891bc39d
|
153
|
encorder.data[1] = current2;
|
nucho |
0:3c49891bc39d
|
154
|
encorderOut.write();
|
nucho |
0:3c49891bc39d
|
155
|
|
nucho |
0:3c49891bc39d
|
156
|
int ctrl1,ctrl2;
|
nucho |
0:3c49891bc39d
|
157
|
ctrl1 = pid_motor1.compute(current1);
|
nucho |
0:3c49891bc39d
|
158
|
ctrl2 = pid_motor2.compute(current2);
|
nucho |
0:3c49891bc39d
|
159
|
|
nucho |
0:3c49891bc39d
|
160
|
motor1.pulsewidth_us(ctrl1+MOTOR_OFFSET);
|
nucho |
0:3c49891bc39d
|
161
|
motor2.pulsewidth_us(ctrl2+MOTOR_OFFSET);
|
nucho |
0:3c49891bc39d
|
162
|
|
nucho |
0:3c49891bc39d
|
163
|
return RTC_OK;
|
nucho |
0:3c49891bc39d
|
164
|
}
|
nucho |
0:3c49891bc39d
|
165
|
|
nucho |
0:3c49891bc39d
|
166
|
|
nucho |
0:3c49891bc39d
|
167
|
//////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
168
|
// on_error
|
nucho |
0:3c49891bc39d
|
169
|
// This function is repeatedly called when
|
nucho |
0:3c49891bc39d
|
170
|
// the RTno is in the ERROR condition.
|
nucho |
0:3c49891bc39d
|
171
|
// The ERROR condition can be recovered,
|
nucho |
0:3c49891bc39d
|
172
|
// when the RTno is reset.
|
nucho |
0:3c49891bc39d
|
173
|
///////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
174
|
int RTno::onError() {
|
nucho |
0:3c49891bc39d
|
175
|
return RTC_OK;
|
nucho |
0:3c49891bc39d
|
176
|
}
|
nucho |
0:3c49891bc39d
|
177
|
|
nucho |
0:3c49891bc39d
|
178
|
////////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
179
|
// This function is called when
|
nucho |
0:3c49891bc39d
|
180
|
// the RTno is reset. If on_reset is
|
nucho |
0:3c49891bc39d
|
181
|
// succeeded, the RTno will enter into
|
nucho |
0:3c49891bc39d
|
182
|
// the INACTIVE condition. If failed
|
nucho |
0:3c49891bc39d
|
183
|
// (return value is RTC_ERROR), RTno
|
nucho |
0:3c49891bc39d
|
184
|
// will stay in ERROR condition.ec
|
nucho |
0:3c49891bc39d
|
185
|
///////////////////////////////////////
|
nucho |
0:3c49891bc39d
|
186
|
int RTno::onReset() {
|
nucho |
0:3c49891bc39d
|
187
|
return RTC_OK;
|
nucho |
0:3c49891bc39d
|
188
|
} |