Mindboards

Code: Select all

float calculateAngle (float posX, float posY){

     //Define the lenghts of the arm links
     float L1 = 18;
     float L2 = 18;

     //Calculate the cosine of the first angle
     float cos2 =  ((posX * posX) + (posY * posY) - (L1 * L1) - (L2 * L2)) / (2 * L1 * L2);

     //Check if the piont is reachable
     if (cos2 >= -1.0 && cos2 <= 1){

     //Calculate both angles in rads
     float radsAngle2 = Acos(cos2);
     float sin2 = Sin(radsAngle2);
     float radsAngle1 = (-(L2 * sin2 * posX) + ((L1 + (L2 * cos2)) * posY))/((L2 * sin2 * posY) + ((L1 + (L2 * cos2)) * posX));

     //Convert angles into degrees
     float degsAngle1 = radsAngle1 * (180/PI);
     float degsAngle2 = radsAngle2 * (180/PI);

     //Return both values
     float Angles[];
     Angles[0] = degsAngle1;
     Angles[1] = degsAngle2;

     return Angles;

     } else {
     TextOut(0,LCD_LINE3, "Point not reachable");
     }
}

void drawPoint(float X, float Y){

     //Define arrays
     float targetJointAng[];
     float currentMotorAng [];
     float currentJointAng[];
     float movementJointAng[];
     float movementMotorAng[];

     targetJointAng = calculateAngle(X,Y);

     //Get the rotation counts of the motors
     currentMotorAng[0] = MotorRotationCount(OUT_A);
     currentMotorAng[1] = MotorRotationCount(OUT_B);

     //Multiply the rotation counts by the gear ratio
     currentJointAng[0] = currentMotorAng[0] * 18.667;
     currentJointAng[1] = currentMotorAng[1] * 18.667;

     //Subtract target and current joint angle
     movementJointAng[0] = targetJointAng[0] - currentJointAng[0];
     movementMotorAng[0] = movementJointAng[0]/ 18.667;

     movementJointAng[1] = targetJointAng[1] - currentJointAng[1];
     movementMotorAng[1] = movementJointAng[1]/ 18.667;

     //Rotate both motors
     RotateMotor(OUT_A ,40,movementMotorAng[0]);
     RotateMotor(OUT_B ,40,movementMotorAng[1]);

}

task main (){

     float Ax = 6;
     float Ay = 10;

     ResetRotationCount(OUT_ABC);

     drawPoint(Ax,Ay);
     Wait(30000);
}

Code: Select all

   
Angles[0] = degsAngle1;
Angles[1] = degsAngle2;

return Angles;

Code: Select all

    //Fatal errors

    #define ERR_ARG      -1
    #define ERR_INSTR    -2
    #define ERR_FILE     -3
    #define ERR_VER      -4
    #define ERR_MEM      -5
    #define ERR_BAD_PTR  -6

    //General errors

    #define ERR_INVALID_PORT   -16
    #define ERR_INVALID_FIELD  -17
    #define ERR_INVALID_QUEUE  -18
    #define ERR_INVALID_SIZE   -19
    #define ERR_NO_PROG        -20

    //Communications specific errors

    #define ERR_COMM_CHAN_NOT_READY -32
    #define ERR_COMM_CHAN_INVALID   -33
    #define ERR_COMM_BUFFER_FULL    -34
    #define ERR_COMM_BUS_ERR        -35

    //Remote control (direct commands) errors

    #define ERR_RC_ILLEGAL_VAL -64
    #define ERR_RC_BAD_PACKET  -65
    #define ERR_RC_UNKNOWN_CMD -66
    #define ERR_RC_FAILED      -67