main.cpp

00001 /*******************************************************************************
00002 This code was developed at ECAM Lyon by Alexandre Simon as an exemple to use the
00003 Poppy_Arm library
00004 *******************************************************************************/
00005 
00006 #include "Poppy_Arm.h"  //Includes the Poppy_Arm library
00007 
00008 Poppy_Arm bras(9600, p9, p10);  //New Poppy_Arm object. Baud rate, Tx and Rx pins
00009 
00010 int main()
00011 {
00012     pc.baud(230400);   //Speeds up the communication between Mbed and pc
00013     double desiredCoord[3] = {80, 80, 200};
00014     //Desired coordinates of the tip of the arm in millimeters
00015 
00016     for (uint8_t i = 1; i <= 6; i++) bras.SetGoalVel(i, 1024 >> 3);
00017     //Reduces the velocity of the motors for security
00018     //for (uint8_t i = 1; i <= 6; i++) bras.SetIGain(i, 20);
00019     //Helps to get a better position but the arm is unstable. Not necessary
00020 
00021     pc.printf("\r\nGoal position :\r\n");
00022     pc.printf("x = %f cm\r\n", desiredCoord[0]/10); //Prints in centimeters
00023     pc.printf("y = %f cm\r\n", desiredCoord[1]/10);
00024     pc.printf("z = %f cm\r\n", desiredCoord[2]/10);
00025 
00026     pc.printf("\r\nCalculating...\r\n");
00027     Timer t;    //Timer to mesure the time to calculate and move
00028     t.start();  //Launches the time measurement
00029     bras.MoveToCoordinates(desiredCoord[0], desiredCoord[1], desiredCoord[2]);
00030     //Moves the tip of the arm to the desired coordinates in millimeters
00031     t.stop();   //Ends the time measurement
00032 
00033     wait(1);    //Waits for the motors to have finished moving
00034     bras.FindCoordinates(); //Finds the current coordinates of the tip of the arm
00035     pc.printf("\r\nActual position :\r\n");
00036     pc.printf("x = %f cm\r\n", bras.GetCoordinates(0)/10);
00037     pc.printf("y = %f cm\r\n", bras.GetCoordinates(1)/10);
00038     pc.printf("z = %f cm\r\n", bras.GetCoordinates(2)/10);
00039     //Prints the current coordinates in cm to compare with the goal position
00040 
00041     pc.printf("\r\nTime to calculate and move = %f s\r\n", t.read());
00042 
00043     while(1) {
00044         //Infinite loop
00045         bras.OpenClamp(true);   //Opens the clamp
00046         bras.ChristmasTree();   //Leds blinks green and red
00047         wait(0.5);              //Waits that the clamp is totally opened
00048         bras.OpenClamp(false);  //Closes the clamp
00049         bras.ChristmasTree();   //Leds blinks green and red
00050         wait(0.5);              //Waits that the clamp is totally closed
00051     }
00052 }