Difference between revisions of "Send PWM data"
Line 42: | Line 42: | ||
<br><br> | <br><br> | ||
− | + | Send PWM by using command_set_torque(). | |
<syntaxhighlight lang="cpp"> | <syntaxhighlight lang="cpp"> | ||
// CAN communication thread | // CAN communication thread |
Revision as of 17:22, 2 July 2019
main.cpp
You can find main.cpp in grasp directory. There are 3 functions you have to edit.
Set pwm values to send
void MainLoop() { bool bRun = true; while (bRun) { int c = Getch(); switch (c) { case 'q': bRun = false; printf("### Turning off.... \n"); break; case '[': printf("[[[[[\n"); vars.pwm_demand[0] = (short)(-100); vars.pwm_demand[4] = (short)(-100); vars.pwm_demand[8] = (short)(-100); break; case ']': printf("]]]]]\n"); vars.pwm_demand[0] = (short)(100); vars.pwm_demand[4] = (short)(100); vars.pwm_demand[8] = (short)(100); break; case 'o': printf("zero torque\n"); vars.pwm_demand[0] = (short)(0); vars.pwm_demand[4] = (short)(0); vars.pwm_demand[8] = (short)(0); break; } } }
Send PWM by using command_set_torque().
// CAN communication thread static void* ioThreadProc(void* inst) { int id; int len; unsigned char data[8]; unsigned char data_return = 0; int i; while (ioThreadRun) { /* wait for the event */ while (0 == get_message(CAN_Ch, &id, &len, data, FALSE)) { switch (id) { case ID_RTR_FINGER_POSE_1: // check CAN protocol on wiki page case ID_RTR_FINGER_POSE_2: case ID_RTR_FINGER_POSE_3: case ID_RTR_FINGER_POSE_4: { int findex = (id & 0x00000007); vars.enc_actual[findex*4 + 0] = (short)(data[0] | (data[1] << 8)); vars.enc_actual[findex*4 + 1] = (short)(data[2] | (data[3] << 8)); vars.enc_actual[findex*4 + 2] = (short)(data[4] | (data[5] << 8)); vars.enc_actual[findex*4 + 3] = (short)(data[6] | (data[7] << 8)); // convert encoder count to joint angle for (i=0; i<MAX_DOF; i++) { q[i] = (double)(vars.enc_actual[i])*(333.3/65536.0)*DEG2RAD; } for (int i=0; i<4;i++) { command_set_torque(CAN_Ch, i, &vars.pwm_demand[4*i]); //usleep(5); } //print pwm for (int i=0; i<4; i++) { printf(">CAN(%d): Joint[%d] PWM : %d %d %d %d\n" , CAN_Ch, i, vars.pwm_demand[i*4+0], vars.pwm_demand[i*4+1], vars.pwm_demand[i*4+2], vars.pwm_demand[i*4+3]); } // print joint angles for (int i=0; i<4; i++) { printf(">CAN(%d): Joint[%d] Pos : %5.1f %5.1f %5.1f %5.1f\n" , CAN_Ch, i, q[i*4+0]*RAD2DEG, q[i*4+1]*RAD2DEG, q[i*4+2]*RAD2DEG, q[i*4+3]*RAD2DEG); } break; } default: printf(">CAN(%d): unknown command %d, len %d\n", CAN_Ch, id, len); } } } return NULL; }
Whos here now: Members 0 Guests 0 Bots & Crawlers 1 |