Poppy_Arm.cpp

00001 #include "Poppy_Arm.h"
00002 
00003 Poppy_Arm::Poppy_Arm(uint32_t baudrate, PinName tx, PinName rx) : XL320(baudrate, tx, rx)
00004 {
00005     dim[0] = 33;
00006     dim[1] = 22;
00007     dim[2] = 55;
00008     dim[3] = 46;
00009     dim[4] = 49;
00010     dim[5] = 128;
00011 }
00012 
00013 uint8_t Poppy_Arm::initNewServo(uint32_t currentBaudRate, uint32_t newBaudRate, uint8_t newId)
00014 {
00015     //Sets the id and the baud rate of all connected motors
00016     if (currentBaudRate != 9600 && currentBaudRate != 57600 && currentBaudRate != 115200 && currentBaudRate != 1000000) {
00017         pc.printf("You did not enter a valid currentBaudRate");
00018         return 1;
00019     }
00020 
00021     if (newBaudRate != 9600 && newBaudRate != 57600 && newBaudRate != 115200 && newBaudRate != 1000000) {
00022         pc.printf("You did not enter a valid newBaudRate");
00023         return 2;
00024     }
00025 
00026     Poppy_Arm s1(currentBaudRate, p9, p10); //Creates a Poppy_Arm object at the current baud rate
00027     switch (newBaudRate) {  //Changes all connected motors baud rate to the new baud rate
00028         case 9600:
00029             s1.SetBaudRate(0xFE, 0);
00030             break;
00031         case 57600:
00032             s1.SetBaudRate(0xFE, 1);
00033             break;
00034         case 115200:
00035             s1.SetBaudRate(0xFE, 2);
00036             break;
00037         case 1000000:
00038             s1.SetBaudRate(0xFE, 3);
00039             break;
00040         default:
00041             return 2;
00042     }
00043     Poppy_Arm s2(newBaudRate, p9, p10);     //Creates a Poppy_Arm object at the new baud rate
00044     s2.SetID(0xFE, newId);                  //Changes all connected motors's id
00045     s2.TurnOnLED(newId , 2);                //Turns on the green led to verify that changes worked
00046     return 0;
00047 }
00048 
00049 void Poppy_Arm::ChristmasTree()
00050 {
00051     //Leds blinks green and red
00052     for (uint8_t i = 1; i <= 6; i++) TurnOnLED(i, 2);
00053     for (uint8_t i = 6; i >= 1; i--) TurnOnLED(i, 1);
00054 }
00055 
00056 void Poppy_Arm::FindCoordinates()
00057 {
00058     //Finds the current coordinates of the tip of the arm
00059     double angle[5];
00060     for (uint8_t i = 0; i <= 4; i++) angle[i] = (double) (0.29 * (GetPresentPos(i + 1) - 512)); //512 units is the 0 degree for calculation
00061     CalculateCoordinates(angle);
00062 }
00063 
00064 void Poppy_Arm::CalculateCoordinates(double angle[])
00065 {
00066     //Calculates the coordinates of the tip of the arm from the motors angles in degree
00067     double co[5], si[5];
00068     for (uint8_t i = 0; i <= 4; i++) {
00069         co[i] = cos(angle[i] * PI / 180);
00070         si[i] = sin(angle[i] * PI / 180);
00071     }
00072     coord[0] = -co[0]*si[1]*dim[2] + dim[3]*(-co[0]*co[1]*si[2] - co[0]*co[2]*si[1]) + dim[4]*(co[3]*(co[0]*co[1]*co[2] - co[0]*si[1]*si[2]) - si[0]*si[3]) + dim[5]*(co[4]*(co[3]*(co[0]*co[1]*co[2] - co[0]*si[1]*si[2]) - si[0]*si[3]) + si[4]*(-co[0]*co[1]*si[2] - co[0]*co[2]*si[1]));
00073     coord[1] = -si[0]*si[1]*dim[2] + dim[3]*(-co[1]*si[0]*si[2] - co[2]*si[0]*si[1]) + dim[4]*(co[0]*si[3] + co[3]*(co[1]*co[2]*si[0] - si[0]*si[1]*si[2])) + dim[5]*(co[4]*(co[0]*si[3] + co[3]*(co[1]*co[2]*si[0] - si[0]*si[1]*si[2])) + si[4]*(-co[1]*si[0]*si[2] - co[2]*si[0]*si[1]));
00074     coord[2] = co[1]*dim[2] + co[3]*dim[4]*(co[1]*si[2] + co[2]*si[1]) + dim[0] + dim[1] + dim[3]*(co[1]*co[2] - si[1]*si[2]) + dim[5]*(co[3]*co[4]*(co[1]*si[2] + co[2]*si[1]) + si[4]*(co[1]*co[2] - si[1]*si[2]));
00075 }
00076 
00077 void Poppy_Arm::MoveToCoordinates(double xCoord, double yCoord, double zCoord)
00078 {
00079     //Moves the tip of the arm to the desired coordinates in millimeters
00080     double angle[5], bestCoord[3] = {0}, bestAngleDeg[5];
00081     uint16_t bestAngle[5];
00082 
00083     if (xCoord == 0) angle[0] = (double) (90); //Moves the first motor in the right direction
00084     else angle[0] = (double) (atan(yCoord / xCoord) * 180 / PI);
00085 
00086     angle[3] = (double) (0); //Motor 4 stays in position 512 to reduce the number of possibilities (512 = 0 degree for calculation)
00087 
00088     for (int16_t i = -90; i <= 90; i = i + 15) { //This range is to avoid that the arm bumps itself
00089         for (int16_t j = -90; j <= 90; j = j + 15) {
00090             for (int16_t k = -90; k <= 90; k = k + 15) {
00091                 angle[1] = (double) (i);
00092                 angle[2] = (double) (j);
00093                 angle[4] = (double) (k);
00094                 CalculateCoordinates(angle);
00095                 if (sqrt(pow(xCoord - coord[0], 2) + pow(yCoord - coord[1], 2) + pow(zCoord - coord[2], 2)) < sqrt(pow(xCoord - bestCoord[0], 2) + pow(yCoord - bestCoord[1], 2) + pow(zCoord - bestCoord[2], 2))) {
00096                     //If the distance between the new coordinates and the desired coordinates is smaller than between the last best coordinates and the desired coordinates then we save the new coordinates
00097                     for (uint8_t l = 0; l <= 2; l++) bestCoord[l] = coord[l];   //Saves the best coordinates
00098                     for (uint8_t l = 0; l <= 4; l++) bestAngleDeg[l] = angle[l]; //Saves the best angles in degrees
00099                     for (uint8_t l = 0; l <= 4; l++) bestAngle[l] = (uint16_t) (512 + angle[l] / 0.29); //Saves the best angles in motor units
00100                 }
00101             }
00102         }
00103     }
00104 
00105     //This part is used to reach more precisely the targeted position. It can be commented
00106     int16_t accuracy = 7; //Accuracy can be changed but calculation time will increase
00107     int16_t lBound[5] = {0, bestAngleDeg[1] - accuracy, bestAngleDeg[2] - accuracy, 0, bestAngleDeg[4] - accuracy};
00108     int16_t uBound[5] = {0, bestAngleDeg[1] + accuracy, bestAngleDeg[2] + accuracy, 0, bestAngleDeg[4] + accuracy};
00109 
00110     for (int16_t i = lBound[1]; i <= uBound[1]; i++) {
00111         for (int16_t j = lBound[2]; j <= uBound[2]; j++) {
00112             for (int16_t k = lBound[4]; k <= uBound[4]; k++) {
00113                 angle[1] = (double) (i);
00114                 angle[2] = (double) (j);
00115                 angle[4] = (double) (k);
00116                 CalculateCoordinates(angle);
00117                 if (sqrt(pow(xCoord - coord[0], 2) + pow(yCoord - coord[1], 2) + pow(zCoord - coord[2], 2)) < sqrt(pow(xCoord - bestCoord[0], 2) + pow(yCoord - bestCoord[1], 2) + pow(zCoord - bestCoord[2], 2))) {
00118                     //If the distance of the new coordinates to the desired coordinates is smaller than the last best coordinates then we save them
00119                     for (uint8_t l = 0; l <= 2; l++) bestCoord[l] = coord[l];   //Saves the best coordinates
00120                     for (uint8_t l = 0; l <= 4; l++) bestAngleDeg[l] = angle[l]; //Saves the best angles in degrees
00121                     for (uint8_t l = 0; l <= 4; l++) bestAngle[l] = (uint16_t) (512 + angle[l] / 0.29); //Saves the best angles in motor units
00122                 }
00123             }
00124         }
00125     }
00126 
00127     pc.putc(' '); //Without this line the arm does not move...
00128     for (uint8_t i = 0; i <= 4; i++) SetGoalPos(i + 1, bestAngle[i]); //Moves the arm
00129 }
00130 
00131 double Poppy_Arm::GetCoordinates(uint8_t Coord) //0 = x, 1 = y, 2 = z
00132 {
00133     //Gets the requested coordinate of the tip of the arm after the execution of FindCoordinates()
00134     switch(Coord) {
00135         case 0 : //x
00136             return coord[0];
00137         case 1 : //y
00138             return coord[1];
00139         case 2 : //z
00140             return coord[2];
00141         default : //Wrong input
00142             pc.printf("Error : Wrong input in GetCoordinates. Out of range (0 to 2)\r\n");
00143             return 32202; //Return "Error" in Leet speak
00144     }
00145 }
00146 
00147 void Poppy_Arm::OpenClamp(bool open)
00148 {
00149     //Opens or closes the clamp
00150     if (open == true) SetGoalPos(6, (uint16_t) (512 - 90 / 0.29));
00151     else SetGoalPos(6, (uint16_t) (512 + 20 / 0.29));
00152 }
00153 
00154 Poppy_Arm::~Poppy_Arm() {};