// Define some lengths and offsets used by the arm #define BaseHeight 110L // (L0)about 120mm #define ShoulderLength 150L // (L1)Not sure yet what to do as the servo is not directly in line, Probably best to offset the angle? // // X is about 140, y is about 40 so sqrt is Hyp is about 155, so maybe about 21 degrees offset #define ShoulderServoOffset 72L // should offset us some... #define ElbowLength 147L //(L2)Length of the Arm from Elbow Joint to Wrist Joint #define WristLength 137L // (L3)Wrist length including Wrist rotate #define G_OFFSET 0 // Offset for static side of gripper? #define IK_FUDGE 5 // How much a fudge between warning and error //============================================================================= // Global Objects //============================================================================= Commander command = Commander(); BioloidControllerEx bioloid = BioloidControllerEx(1000000); WrapperSerial Serial = WrapperSerial(); //============================================================================= // Global Variables... //============================================================================= boolean g_fArmActive = false; // Is the arm logically on? byte g_bIKMode = IKM_IK3D_CARTESIAN; // Which mode of operation are we in... uint8_t g_bIKStatus = IKS_SUCCESS; // Status of last call to DoArmIK; boolean g_fServosFree = true; // Current IK values int g_sIKGA; // IK Gripper angle.. int g_sIKX; // Current X value in mm int g_sIKY; //
int main(int argc, char** argv){
Commander commander = Commander();
commander.readInstructions();
printf(commander.executeInstructions( ).c_str( ));
return 0;
}