Line 3: Line 3:
 
<br><br>
 
<br><br>
  
Initialize CAN I/O thread and start.
+
Set pwm values to send
 
<syntaxhighlight lang="cpp">
 
<syntaxhighlight lang="cpp">
 
void MainLoop()
 
void MainLoop()
Line 75: Line 75:
 
                 {
 
                 {
 
                     q[i] = (double)(vars.enc_actual[i])*(333.3/65536.0)*DEG2RAD;
 
                     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]);
 
                 }
 
                 }
  
Line 92: Line 104:
 
     }
 
     }
 
     return NULL;
 
     return NULL;
}
 
</syntaxhighlight>
 
<br><br>
 
 
CloseCAN()
 
<syntaxhighlight lang="cpp">
 
// Close CAN data channel
 
void CloseCAN()
 
{
 
    printf(">CAN: stop periodic communication\n");
 
   
 
    //close CAN listening thread
 
    if (ioThreadRun)
 
    {
 
        printf(">CAN: stoped listening CAN frames\n");
 
        ioThreadRun = false;
 
        int status;
 
        pthread_join(hThread, (void **)&status);
 
        hThread = 0;
 
    }
 
 
    // CAN close
 
    printf(">CAN(%d): close\n", CAN_Ch);
 
    int ret = command_can_close(CAN_Ch);
 
    if(ret < 0) printf("ERROR command_can_close !!! \n");
 
 
}
 
}
 
</syntaxhighlight>
 
</syntaxhighlight>
 
<br><br>
 
<br><br>

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;
        }
 
    }
}



Get CAN raw data and identify data to get position values.

// 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