Skip to content

Commit f522be0

Browse files
ywen3ywen3
committed
update M1DemoSates
1 parent c81814c commit f522be0

2 files changed

Lines changed: 94 additions & 56 deletions

File tree

‎src/apps/M1DemoMachine/M1DemoStates.cpp‎

Lines changed: 85 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@ void Calibration::entry(void) {
3636
}
3737

3838
void Calibration::during(void) {
39+
// Two stage calibration
3940
if(stages == 1){
4041
// Stage 1: Position Homing (zeroing)
4142
dq=robot->getJointVel();
@@ -59,8 +60,7 @@ void Calibration::during(void) {
5960
robot->printJointStatus();
6061
}
6162
}
62-
else
63-
{
63+
else {
6464
// Stage 2: Force Sensor Zeroing (calibration)
6565
q(0) = 16; // 16 is vertical for #2
6666
setMovementReturnCode_t result = robot->setJointPos(q);
@@ -104,14 +104,6 @@ void Monitoring::during(void) {
104104
robot->printJointStatus();
105105
std::cout << std::dec << iterations << ": " << std::setprecision(2) << tau(0) << std::endl;
106106
}
107-
108-
// tau = robot->getJointTor_s();
109-
// tau(0) = tau(0)*10;
110-
//// robot->setJointTor(tau);
111-
//// robot->setJointTor_comp(tau);
112-
// if(robot->setJointTor_comp(tau) != SUCCESS){
113-
// std::cout << "Error: " << std::endl;
114-
// }
115107
}
116108

117109
void Monitoring::exit(void) {
@@ -121,21 +113,21 @@ void Monitoring::exit(void) {
121113

122114
//******************************* Demo state **************************
123115
void M1PositionTracking::entryCode(void) {
124-
std::cout << "Enter Position tracking!" << std::endl;
116+
std::cout << "Enter demos!" << std::endl;
125117
cfreq = 800; // set control loop frequency
126-
mode = 3; // Set mode to 1 for position control test: move from 0 to 90 degree
118+
mode = 1; // Set mode to 1 for position control test: move from 0 to 90 degree
127119
// set mode to 2 for velocity control test
128120
// set mode to 3 for torque control test
129121
// set mode to 4 for admittance control
130-
sub_mode = 3;
122+
sub_mode = 1;
131123
// sub_mode 1 for sine wave tracking
132124
// sub_mode 2 for ramp tracking
133125
// sub_mode 3 for torque control only
134126
robot->applyCalibration();
135127
// robot->initPositionControl();
136128
// q(0) = 50;
137129
// robot->setJointPos(q);
138-
// sleep(1);
130+
sleep(1);
139131
cycle = 0;
140132
counter = 0;
141133
sflag = 0;
@@ -144,7 +136,7 @@ void M1PositionTracking::entryCode(void) {
144136
}
145137

146138
void M1PositionTracking::duringCode(void) {
147-
if(iterations%100==1) {
139+
if(iterations%1000==1) {
148140
//std::cout << "Doing nothing for "<< elapsedTime << "s..." << std::endl;
149141
robot->printJointStatus();
150142
}
@@ -157,13 +149,16 @@ void M1PositionTracking::duringCode(void) {
157149
control(mode);
158150
if(cycle >= 4)
159151
{
160-
//mode = mode + 1;
152+
exitCode();
153+
std::cout << "End demo!" << std::endl;
154+
mode = mode + 1;
161155
cycle = 0;
162156
counter = 0;
163-
step = step + 0.05; // slope torque
164-
magnitude = magnitude + 0.2; //sine wave torque
165-
freq = freq + 0.1; //sine wave torque
166-
//initMode(mode);
157+
sflag = 0;
158+
// step = step + 0.05; // slope torque
159+
// magnitude = magnitude + 0.2; //sine wave torque
160+
// freq = freq + 0.1; //sine wave torque
161+
initMode(mode);
167162
// if(magnitude > 5)
168163
// {
169164
// mode = 4;
@@ -200,25 +195,29 @@ void M1PositionTracking::initMode(int mode_t){
200195
mode = mode_t;
201196
switch(mode_t){
202197
case 1:
198+
std::cout << "Demo position tracking!" << std::endl;
203199
robot->initPositionControl();
204-
freq = 0.5;
200+
freq = 0.1;
205201
counter = 1;
206-
magnitude = 20;
202+
magnitude = 10;
207203
break;
208204
case 2:
205+
std::cout << "Demo velocity tracking!" << std::endl;
209206
robot->initVelocityControl();
210207
// robot->admittanceControl();
211-
freq = 0.2;
208+
freq = 0.5;
212209
magnitude = 20; // degree per second
213210
break;
214211
case 3:
212+
std::cout << "Demo torque tracking!" << std::endl;
215213
robot->initTorqueControl();
216-
freq = 0.1;
217-
magnitude = 3; // magnitude for sine wave without compensation
214+
freq = 0.5;
215+
magnitude = 0.6; // magnitude for sine wave without compensation
218216
// magnitude = 0.6; // magnitude for sine wave
219217
step = 0.1;
220218
break;
221219
case 4:
220+
std::cout << "Demo admittance control!" << std::endl;
222221
robot->initVelocityControl();
223222
robot->m1ForceSensor->calibrate();
224223
Ks = 0;
@@ -254,10 +253,21 @@ void M1PositionTracking::control(int mode_t){
254253
void M1PositionTracking::positionControl(void){
255254
// position control;
256255
q=robot->getJointPos();
257-
std::cout << q(0) << " <-> ";
256+
// std::cout << q(0) << " <-> ";
258257
switch(sub_mode){
259258
case 1: // sine wave position command
260-
q(0) = magnitude*sin(2*M_PI*freq*iterations/100);
259+
sflag = sin(2*M_PI*freq*iterations/100);
260+
q(0) = magnitude*sflag;
261+
if(abs(sflag)<10e-8 && dir==true)
262+
{
263+
dir = false;
264+
}
265+
else if(abs(sflag)<10e-8 && dir==false)
266+
{
267+
dir = true;
268+
cycle = cycle + 1;
269+
std::cout << std::setprecision(2) << "position magnitude: " << magnitude << "; frequency: " << freq << "; Cycle: "<< cycle << std::endl;
270+
}
261271
break;
262272
case 2:
263273
if(dir) // positive direction
@@ -289,7 +299,7 @@ void M1PositionTracking::positionControl(void){
289299
}
290300

291301
// display and set joint position
292-
std::cout << q(0) << std::endl;
302+
// std::cout << q(0) << std::endl;
293303
if(robot->setJointPos(q) != SUCCESS){
294304
std::cout << "Error: " << std::endl;
295305
}
@@ -301,33 +311,54 @@ void M1PositionTracking::velocityControl(void){
301311
// velocity control, differential velocity and command velocity
302312
q=robot->getJointPos();
303313

304-
if(dir) // positive direction
305-
{
306-
if(q(0) < 90)
307-
{
308-
dq(0) = magnitude;
309-
}
310-
else
311-
{
312-
dq(0) = 0;
313-
dir = false;
314-
}
315-
}
316-
else // negative direction
317-
{
318-
if(q(0) > 0)
319-
{
320-
dq(0) = -magnitude;
321-
}
322-
else
323-
{
324-
dq(0) = 0;
325-
dir = true;
326-
magnitude = magnitude + 10;
327-
cycle = cycle + 1;
328-
}
314+
switch(sub_mode){
315+
case 1: // sine wave velocity command
316+
sflag = sin(2*M_PI*freq*iterations/100);
317+
dq(0) = magnitude*sflag;
318+
if(abs(sflag)<10e-8 && dir==true)
319+
{
320+
dir = false;
321+
}
322+
else if(abs(sflag)<10e-8 && dir==false)
323+
{
324+
dir = true;
325+
cycle = cycle + 1;
326+
std::cout << std::setprecision(2) << "velocity magnitude: " << magnitude << "; frequency: " << freq << "; Cycle: "<< cycle << std::endl;
327+
}
328+
break;
329+
case 2:
330+
if(dir) // positive direction
331+
{
332+
if(q(0) < 90)
333+
{
334+
dq(0) = magnitude;
335+
}
336+
else
337+
{
338+
dq(0) = 0;
339+
dir = false;
340+
}
341+
}
342+
else // negative direction
343+
{
344+
if(q(0) > 0)
345+
{
346+
dq(0) = -magnitude;
347+
}
348+
else
349+
{
350+
dq(0) = 0;
351+
dir = true;
352+
magnitude = magnitude + 10;
353+
cycle = cycle + 1;
354+
}
355+
}
356+
break;
357+
default:
358+
std::cout << "Wrong sub mode !" << std::endl;
329359
}
330360

361+
331362
// give velocity command for 5 s and check the position changes
332363
// if ((iterations > 0) & (iterations <= 500)) {
333364
// dq(0) = magnitude;
@@ -441,7 +472,7 @@ void M1PositionTracking::torqueControl(void){
441472
// }
442473
// tau_cmd(0) = 0;
443474
if(robot->setJointTor_comp(tau_cmd, tau_cmd, ff_ratio) != SUCCESS){
444-
std::cout << "Error: " << std::endl;
475+
std::cout << "Error: " << tau_cmd << std::endl;
445476
}
446477
// if(robot->setJointTor(tau_cmd) != SUCCESS){
447478
// std::cout << "Error: " << std::endl;

‎src/hardware/drives/KincoDrive.cpp‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -95,6 +95,7 @@ bool KincoDrive::resetError(){
9595

9696
bool KincoDrive::initPDOs() {
9797
spdlog::debug("KincoDrive::initPDOs");
98+
9899
spdlog::debug("Set up STATUS_WORD TPDO on Node {}", NodeID);
99100
int TPDO_Num = 1;
100101
if (sendSDOMessages(generateTPDOConfigSDO(TPDO_MappedObjects[TPDO_Num], TPDO_Num, TPDO_COBID[TPDO_Num] + NodeID, 0xFF)) < 0) {
@@ -122,12 +123,14 @@ bool KincoDrive::initPDOs() {
122123
spdlog::error("Set up DIGITAL_IN TPDO FAILED on node {}", NodeID);
123124
return false;
124125
}
126+
125127
spdlog::debug("Set up DIGITAL_OUT RPDO on Node {}", NodeID);
126128
int RPDO_Num = 1;
127129
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff, 14)) < 0) {
128-
spdlog::error("Set up DIGITAL_OUT RPDO FAILED on node {}", NodeID);
129-
return false;
130+
spdlog::error("Set up DIGITAL_OUT RPDO FAILED on node {}", NodeID);
131+
return false;
130132
}
133+
131134
// Calculate COB_ID. If RPDO:
132135
//int COB_ID = 0x100 * (PDO_Num+1) + NodeID;
133136
//spdlog::debug("Set up CONTROL_WORD RPDO on Node {}", NodeID);
@@ -136,24 +139,28 @@ bool KincoDrive::initPDOs() {
136139
// spdlog::error("Set up CONTROL_WORD RPDO FAILED on node {}", NodeID);
137140
// return false;
138141
//}
142+
139143
spdlog::debug("Set up TARGET_POS RPDO on Node {}", NodeID);
140144
RPDO_Num = 2;
141145
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff)) < 0) {
142146
spdlog::error("Set up TARGET_POS RPDO FAILED on node {}", NodeID);
143147
return false;
144148
}
149+
145150
spdlog::debug("Set up TARGET_VEL RPDO on Node {}", NodeID);
146151
RPDO_Num = 3;
147152
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff)) < 0) {
148153
spdlog::error("Set up ARGET_VEL RPDO FAILED on node {}", NodeID);
149154
return false;
150155
}
156+
151157
spdlog::debug("Set up TARGET_TOR RPDO on Node {}", NodeID);
152158
RPDO_Num = 4;
153159
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff, 0x08)) < 0) {
154160
spdlog::error("Set up TARGET_TOR RPDO FAILED on node {}", NodeID);
155161
return false;
156162
}
163+
157164
return true;
158165
}
159166

0 commit comments

Comments
 (0)