Difference between revisions of "Send PWM data"

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

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

// 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");
}