Ben Katz
/
Teleop_Controller
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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) {
-
+
}