Ben Katz
bilateral feedback demo
Fork of
CanMasterTest
by 
Ben Katz
Revision 3:d8e431e1d364, committed 2020-10-21
- Comitter:
- benkatz
- Date:
- Wed Oct 21 00:00:39 2020 +0000
- Parent:
- 2:25837cbaee98
- Commit message:
- teleop controller;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Nov 29 17:48:13 2017 +0000
+++ b/main.cpp Wed Oct 21 00:00:39 2020 +0000
@@ -63,7 +63,7 @@
float scaling = 0;
-int control_mode = 0;
+int control_mode = 1;
/// Value Limits ///
#define P_MIN -12.5f
@@ -238,6 +238,7 @@
void WriteAll(){
//toggle = 1;
+ wait(.0001);
can1.write(abad1);
wait(.0001);
can2.write(abad2);
@@ -283,6 +284,18 @@
//Joint Coupling
KD1[0] = 0; KD1[1] = 0; KD1[2] = 0;
KD2[0] = 0; KD2[1] = 0; KD2[2] = 0;
+ float deltaq1 = q2[0] - q1[0];
+ float deltaq2 = q2[1] - q1[1];
+ float deltaq3 = q2[2] - q1[2];
+ /*
+ tau1[0] = -scaling*(kp_q*(deltaq1 + 1000.0f*deltaq1*abs(deltaq1)) + kd_q*(dq2[0] - dq1[0]));
+ tau2[0] = -scaling*(kp_q*(-(deltaq1 + 1000.0f*deltaq1*abs(deltaq1))) + kd_q*(dq1[0] - dq2[0]));
+ tau1[1] = scaling*(kp_q*(deltaq2+1000.0f*deltaq2*abs(deltaq2)) + kd_q*(dq2[1] - dq1[1]));
+ tau2[1] = scaling*(kp_q*(-(deltaq2+1000.0f*deltaq2*abs(deltaq2))) + kd_q*(dq1[1] - dq2[1]));
+ tau1[2] = -scaling*((kp_q/1.5f)*(deltaq3+1000.0f*deltaq3*abs(deltaq3)) + (kd_q/2.25f)*(dq2[2] - dq1[2]));
+ tau2[2] = -scaling*((kp_q/1.5f)*(-(deltaq3+1000.0f*deltaq3*abs(deltaq3))) + (kd_q/2.25f)*(dq1[2] - dq2[2]));
+ */
+
tau1[0] = -scaling*(kp_q*(q2[0] - q1[0]) + kd_q*(dq2[0] - dq1[0]));
tau2[0] = -scaling*(kp_q*(q1[0] - q2[0]) + kd_q*(dq1[0] - dq2[0]));
tau1[1] = scaling*(kp_q*(q2[1] - q1[1]) + kd_q*(dq2[1] - dq1[1]));
@@ -296,6 +309,8 @@
pack_cmd(&hip2, 0, 0, 0, KD2[1]+.005f, tau2[1]);
pack_cmd(&knee1, 0, 0, 0, KD1[2]+.0033f, tau1[2]);
pack_cmd(&knee2, 0, 0, 0, KD2[2]+.0033f, tau2[2]);
+
+ //printf("%f %f\n\r", tau1[1], 10.0f*deltaq2*abs(deltaq2));
}
break;
@@ -319,22 +334,22 @@
float kx2 = wn_des/M2[0][0];
kx1 = fminf(kmax, kx1);
kx2 = fminf(kmax, kx2);
- f1[0] = scaling*(kx1*(xlim - p1[0]) + 0.0f*kd*(0 - v1[0]))*contact1[0];
- f2[0] = scaling*(kx2*(xlim - p2[0]) + 0.0f*kd*(0 - v2[0]))*contact2[0];
+ f1[0] = scaling*(kx1*(xlim - p1[0]) + 0.03f*kd*(0 - v1[0]))*contact1[0];
+ f2[0] = scaling*(kx2*(xlim - p2[0]) + 0.03f*kd*(0 - v2[0]))*contact2[0];
float ky1 = wn_des/M1[1][1];
float ky2 = wn_des/M2[1][1];
ky1 = fminf(kmax, ky1);
ky2 = fminf(kmax, ky2);
- f1[1] = scaling*(ky1*(ylim - p1[1]) + 0.0f*kd*(0 - v1[1]))*contact1[1];
- f2[1] = scaling*(ky2*(ylim - p2[1]) + 0.0f*kd*(0 - v2[1]))*contact2[1];
+ f1[1] = scaling*(ky1*(ylim - p1[1]) + 0.03f*kd*(0 - v1[1]))*contact1[1];
+ f2[1] = scaling*(ky2*(ylim - p2[1]) + 0.03f*kd*(0 - v2[1]))*contact2[1];
float kz1 = wn_des/M1[2][2];
float kz2 = wn_des/M2[2][2];
kz1 = fminf(kmax, kz1);
kz2 = fminf(kmax, kz2);
- f1[2] = scaling*(kz1*(zlim - p1[2]) + 0.0f*kd*(0 - v1[2]))*contact1[2];
- f2[2] = scaling*(kz2*(zlim - p2[2]) + 0.0f*kd*(0 - v2[2]))*contact2[2];
+ f1[2] = scaling*(kz1*(zlim - p1[2]) + 0.03f*kd*(0 - v1[2]))*contact1[2];
+ f2[2] = scaling*(kz2*(zlim - p2[2]) + 0.03f*kd*(0 - v2[2]))*contact2[2];
//
tau1[0] = -1*(f1[0]*J1[0][0] + f1[1]*J1[1][0] + f1[2]*J1[2][0]);
@@ -344,12 +359,12 @@
tau1[2] = -1*(f1[0]*J1[0][2] + f1[1]*J1[1][2] + f1[2]*J1[2][2]);
tau2[2] = -1*(f2[0]*J2[0][2] + f2[1]*J2[1][2] + f2[2]*J2[2][2]);
- KD1[0] = 0.4f*(kd*scaling)*(contact1[0]*J1[0][0]*J1[0][0] + contact1[1]*J1[1][0]*J1[1][0] + contact1[2]*J1[2][0]*J1[2][0]);
- KD2[0] = 0.4f*(kd*scaling)*(contact2[0]*J2[0][0]*J2[0][0] + contact2[1]*J2[1][0]*J2[1][0] + contact2[2]*J2[2][0]*J2[2][0]);
- KD1[1] = 0.4f*(kd*scaling)*(contact1[0]*J1[0][1]*J1[0][1] + contact1[1]*J1[1][1]*J1[1][1] + contact1[2]*J1[2][1]*J1[2][1]);
- KD2[1] = 0.4f*(kd*scaling)*(contact2[0]*J2[0][1]*J2[0][1] + contact2[1]*J2[1][1]*J2[1][1] + contact2[2]*J2[2][1]*J2[2][1]);
- KD1[2] = 0.4f*0.44f*(kd*scaling)*(contact1[0]*J1[0][2]*J1[0][2] + contact1[1]*J1[1][2]*J1[1][2] + contact1[2]*J1[2][2]*J1[2][2]);
- KD2[2] = 0.4f*0.44f*(kd*scaling)*(contact2[0]*J2[0][2]*J2[0][2] + contact2[1]*J2[1][2]*J2[1][2] + contact2[2]*J2[2][2]*J2[2][2]);
+ KD1[0] = 0.01f*(kd*scaling)*(contact1[0]*J1[0][0]*J1[0][0] + contact1[1]*J1[1][0]*J1[1][0] + contact1[2]*J1[2][0]*J1[2][0]);
+ KD2[0] = 0.01f*(kd*scaling)*(contact2[0]*J2[0][0]*J2[0][0] + contact2[1]*J2[1][0]*J2[1][0] + contact2[2]*J2[2][0]*J2[2][0]);
+ KD1[1] = 0.01f*(kd*scaling)*(contact1[0]*J1[0][1]*J1[0][1] + contact1[1]*J1[1][1]*J1[1][1] + contact1[2]*J1[2][1]*J1[2][1]);
+ KD2[1] = 0.01f*(kd*scaling)*(contact2[0]*J2[0][1]*J2[0][1] + contact2[1]*J2[1][1]*J2[1][1] + contact2[2]*J2[2][1]*J2[2][1]);
+ KD1[2] = 0.01f*0.44f*(kd*scaling)*(contact1[0]*J1[0][2]*J1[0][2] + contact1[1]*J1[1][2]*J1[1][2] + contact1[2]*J1[2][2]*J1[2][2]);
+ KD2[2] = 0.01f*0.44f*(kd*scaling)*(contact2[0]*J2[0][2]*J2[0][2] + contact2[1]*J2[1][2]*J2[1][2] + contact2[2]*J2[2][2]*J2[2][2]);
pack_cmd(&abad1, 0, 0, 0, KD1[0]+.005f, tau1[0]);
pack_cmd(&abad2, 0, 0, 0, KD2[0]+.005f, tau2[0]);
@@ -383,7 +398,7 @@
if(counter>100){
//tcmd = -1*tcmd;
- //printf("%.4f %.4f \n\r", q1[1], q2[1]);
+ printf("%.3f %.3f %.3f %.3f %.3f %.3f \n\r", q1[0], q1[1], q1[2], q2[0], q2[1], q2[2]);
//printf("%f\n\r", scaling);
counter = 0 ;
}
@@ -417,7 +432,7 @@
msg->data[5] = 0xFF;
msg->data[6] = 0xFF;
msg->data[7] = 0xFE;
- WriteAll();
+ //WriteAll();
}
void EnterMotorMode(CANMessage * msg){
@@ -429,7 +444,7 @@
msg->data[5] = 0xFF;
msg->data[6] = 0xFF;
msg->data[7] = 0xFC;
- WriteAll();
+ //WriteAll();
}
void ExitMotorMode(CANMessage * msg){
@@ -462,17 +477,20 @@
case('m'):
printf("\n\r entering motor mode \n\r");
EnterMotorMode(&abad1);
- Zero(&abad1);
EnterMotorMode(&abad2);
- Zero(&abad2);
EnterMotorMode(&hip1);
- Zero(&hip1);
EnterMotorMode(&hip2);
+ EnterMotorMode(&knee1);
+ EnterMotorMode(&knee2);
+ WriteAll();
+
+ Zero(&abad1);
+ Zero(&abad2);
+ Zero(&hip1);
Zero(&hip2);
- EnterMotorMode(&knee1);
Zero(&knee1);
- EnterMotorMode(&knee2);
Zero(&knee2);
+ WriteAll();
wait(.5);
enabled = 1;
loop.attach(&sendCMD, .001);
@@ -480,11 +498,17 @@
case('z'):
printf("\n\r zeroing \n\r");
Zero(&abad1);
+ can1.write(abad1);
Zero(&abad2);
+ can2.write(abad2);
Zero(&hip1);
+ can1.write(hip1);
Zero(&hip2);
+ can2.write(hip2);
Zero(&knee1);
+ can1.write(knee1);
Zero(&knee2);
+ can2.write(knee2);
break;
case('0'):
control_mode = 0;
@@ -550,12 +574,21 @@
EnterMotorMode(&hip2);
EnterMotorMode(&knee1);
EnterMotorMode(&knee2);
- Zero(&knee2);
- Zero(&knee1);
- Zero(&hip1);
- Zero(&hip2);
- Zero(&abad2);
- Zero(&abad1);
+ WriteAll();
+ wait(.1);
+Zero(&abad1);
+ can1.write(abad1);
+ Zero(&abad2);
+ can2.write(abad2);
+ Zero(&hip1);
+ can1.write(hip1);
+ Zero(&hip2);
+ can2.write(hip2);
+ Zero(&knee1);
+ can1.write(knee1);
+ Zero(&knee2);
+ can2.write(knee2);
+
@@ -563,7 +596,7 @@
enabled = 1;
loop.attach(&sendCMD, .001);
while(1) {
-
+
}