Giancarlo R. / Mbed 2 deprecated Arm_Serial_Coordinates

Takes in serial coordinates (in mm) and moves servos in Lynxmotion AL5 arm accordingly.

Dependencies:   mbed Servo

main.cpp@0:4a15e2d20446, 2011-12-14 (annotated)

Committer:
gvalentin3
Date:
Wed Dec 14 17:08:38 2011 +0000
Revision:
0:4a15e2d20446
First published version. Height is still hardcoded to -70mm.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gvalentin3 0:4a15e2d20446 1 #include "mbed.h"
gvalentin3 0:4a15e2d20446 2 #include "robo.h"
gvalentin3 0:4a15e2d20446 3
gvalentin3 0:4a15e2d20446 4 #define PI 3.14159265
gvalentin3 0:4a15e2d20446 5 #include "mbed.h"
gvalentin3 0:4a15e2d20446 6
gvalentin3 0:4a15e2d20446 7 robo arm(p21, p22, p23, p24, p25, p26);
gvalentin3 0:4a15e2d20446 8 Serial myserial(p28, p27);
gvalentin3 0:4a15e2d20446 9
gvalentin3 0:4a15e2d20446 10 float deg = 180/PI;
gvalentin3 0:4a15e2d20446 11 float rad = PI/180;
gvalentin3 0:4a15e2d20446 12 float openclaw = 0;
gvalentin3 0:4a15e2d20446 13 float closeclaw = 100;
gvalentin3 0:4a15e2d20446 14
gvalentin3 0:4a15e2d20446 15 char xin;
gvalentin3 0:4a15e2d20446 16 char yin;
gvalentin3 0:4a15e2d20446 17 char cords[3];
gvalentin3 0:4a15e2d20446 18 int x;
gvalentin3 0:4a15e2d20446 19 int y;
gvalentin3 0:4a15e2d20446 20 int z;
gvalentin3 0:4a15e2d20446 21 int zin;
gvalentin3 0:4a15e2d20446 22
gvalentin3 0:4a15e2d20446 23 int main() {
gvalentin3 0:4a15e2d20446 24
gvalentin3 0:4a15e2d20446 25 //Start Arm in standard position.//
gvalentin3 0:4a15e2d20446 26 arm.writeBase(90*rad);
gvalentin3 0:4a15e2d20446 27 wait(0.2);
gvalentin3 0:4a15e2d20446 28 arm.writeElbow(90*rad);
gvalentin3 0:4a15e2d20446 29 wait(0.2);
gvalentin3 0:4a15e2d20446 30 arm.writeWrist(90*rad);
gvalentin3 0:4a15e2d20446 31 wait(0.2);
gvalentin3 0:4a15e2d20446 32 arm.writeClaw(openclaw);
gvalentin3 0:4a15e2d20446 33 wait(0.2);
gvalentin3 0:4a15e2d20446 34
gvalentin3 0:4a15e2d20446 35
gvalentin3 0:4a15e2d20446 36 while (1) {
gvalentin3 0:4a15e2d20446 37
gvalentin3 0:4a15e2d20446 38 myserial.scanf("%s", &cords); //scan a string with the coordinates of the object;
gvalentin3 0:4a15e2d20446 39
gvalentin3 0:4a15e2d20446 40 xin=cords[0];
gvalentin3 0:4a15e2d20446 41 zin=cords[1];
gvalentin3 0:4a15e2d20446 42
gvalentin3 0:4a15e2d20446 43 x= (int)xin; //x will be distance sent by amigobot's camera. (It is y in the image, but x in the algorithm)
gvalentin3 0:4a15e2d20446 44 y= -70; //height form the base of the arm to the top of the trash in mm. Right now it is hardcoded.
gvalentin3 0:4a15e2d20446 45 z= (int)zin; //z will be the horizontal distance. Right now its not being used.
gvalentin3 0:4a15e2d20446 46
gvalentin3 0:4a15e2d20446 47 float * angles; //Initialize angles vector where the calculatiosn will be stored.
gvalentin3 0:4a15e2d20446 48 angles = (float *)malloc(3 * sizeof(float));
gvalentin3 0:4a15e2d20446 49 angles[0] = 0;
gvalentin3 0:4a15e2d20446 50 angles[1] = 0;
gvalentin3 0:4a15e2d20446 51 angles[2] = 0;
gvalentin3 0:4a15e2d20446 52
gvalentin3 0:4a15e2d20446 53 float oe = -90*rad;
gvalentin3 0:4a15e2d20446 54 angles = arm.calculateangle(x, y, oe, angles);// convert positions to angles and store in angles variable
gvalentin3 0:4a15e2d20446 55
gvalentin3 0:4a15e2d20446 56 //store the angles into a printable variable (Haven't had luck printing array entries)
gvalentin3 0:4a15e2d20446 57 float baseangle = angles[0];
gvalentin3 0:4a15e2d20446 58 float elbowangle = angles[1];
gvalentin3 0:4a15e2d20446 59 float wristangle = angles[2];
gvalentin3 0:4a15e2d20446 60
gvalentin3 0:4a15e2d20446 61 // Pick up Object
gvalentin3 0:4a15e2d20446 62 arm.writeBase(baseangle); //move arm based on calculated angles
gvalentin3 0:4a15e2d20446 63 wait(0.05);
gvalentin3 0:4a15e2d20446 64 arm.writeElbow(elbowangle); //move arm based on calculated angles
gvalentin3 0:4a15e2d20446 65 wait(0.05);
gvalentin3 0:4a15e2d20446 66 arm.writeWrist(wristangle); //move arm based on calculated angles
gvalentin3 0:4a15e2d20446 67 wait(1.5);
gvalentin3 0:4a15e2d20446 68 arm.writeClaw(closeclaw); //close the claw before moving to object
gvalentin3 0:4a15e2d20446 69 wait(1);
gvalentin3 0:4a15e2d20446 70
gvalentin3 0:4a15e2d20446 71 // Deposit Object in bin behind it
gvalentin3 0:4a15e2d20446 72 arm.writeBase(70*rad);
gvalentin3 0:4a15e2d20446 73 wait(0.2);
gvalentin3 0:4a15e2d20446 74 arm.writeElbow(0*rad);
gvalentin3 0:4a15e2d20446 75 wait(0.7);
gvalentin3 0:4a15e2d20446 76 arm.writeElbow(-90*rad);
gvalentin3 0:4a15e2d20446 77 wait(0.7);
gvalentin3 0:4a15e2d20446 78 arm.writeWrist(0*rad);
gvalentin3 0:4a15e2d20446 79 wait(1);
gvalentin3 0:4a15e2d20446 80
gvalentin3 0:4a15e2d20446 81 // Let go of Object
gvalentin3 0:4a15e2d20446 82 arm.writeClaw(openclaw);
gvalentin3 0:4a15e2d20446 83 wait(1.5);
gvalentin3 0:4a15e2d20446 84
gvalentin3 0:4a15e2d20446 85 //Return to initial position
gvalentin3 0:4a15e2d20446 86 arm.writeBase(90*rad);
gvalentin3 0:4a15e2d20446 87 wait(0.2);
gvalentin3 0:4a15e2d20446 88 arm.writeElbow(0*rad);
gvalentin3 0:4a15e2d20446 89 wait(1);
gvalentin3 0:4a15e2d20446 90 arm.writeElbow(90*rad);
gvalentin3 0:4a15e2d20446 91 wait(0.2);
gvalentin3 0:4a15e2d20446 92 arm.writeWrist(90*rad);
gvalentin3 0:4a15e2d20446 93 wait(0.2);
gvalentin3 0:4a15e2d20446 94 //free(angles);
gvalentin3 0:4a15e2d20446 95 }//end of while
gvalentin3 0:4a15e2d20446 96 }//end main