Aimen Al-Refai / Mbed 2 deprecated 4dofRoboticArmAX12

The intent of this project is to use and learn, inverse kinematics, pid control of position, touch control using smartgpu_v1 board for viewing data or debug messages while moving a 4 DOF robotic arm.

Dependencies:   AX12 InversKinem PIDPositionController SMARTGPU SerialHalfDuplex mbed

http://mbed.org/users/aimen/notebook/4dofroboticarmax12_lpc1768-with-smartgpu/

Files at this revision

API Documentation at this revision

Revision 3:41b605f9cdd5, committed 2014-04-07

Comitter:
aimen
Date:
Mon Apr 07 06:09:27 2014 +0000
Parent:
2:de8b38a39938
Commit message:
implemented manual touch control of individual AX_12A servos

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Tue Apr 01 07:29:17 2014 +0000
+++ b/main.cpp	Mon Apr 07 06:09:27 2014 +0000
@@ -4,6 +4,8 @@
 
 
 SMARTGPU lcd(p28,p27,p26);        //(TX,RX,Reset);
+//Each time we use the touchscreen we must define a int array that stores the X and Y readed or touched coordinates.
+int touch[2];
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
 DigitalOut myled3(LED3);
@@ -19,30 +21,120 @@
   lcd.reset();                    //physically reset SMARTGPU
   lcd.start();                    //initialize the SMARTGPU processor
   lcd.baudChange(1000000);        //set high baud for fast drawing
-
-  while(1) {
+  
+  //Draw a cover
+  lcd.drawTriangle(20,20,70,40,40,80,YELLOW,FILL);
+  lcd.drawRectangle(250,30,300,80,MAGENTA,FILL);
+  lcd.drawCircle(50,190,28,CYAN,FILL);
+  lcd.drawTriangle(270,230,240,190,300,170,RED,FILL);
+  lcd.string(10,70,319,239,WHITE,FONT7,TRANS,"4 DOF Robot Control");
+  lcd.string(35,130,310,220,GREEN,FONT4,TRANS,"Touch screen to begin");
+  wait_ms(1000);
+  char buffer[100]; //debug buffer
+  while(!lcd.touchScreen(touch)); // Wait for a touch on the screen to start
+  lcd.erase();
+  
+  //AX counters for manual controls
+  int cntr_1 = 150;
+  int cntr_2 = 226;
+  int cntr_3 = 9;
+  int cntr_4 = 90;
+  int cntr_5 = 150;
+  //set Arm to starting folded position
+  myax12_1.SetGoal(cntr_1);
+  myax12_2.SetGoal(cntr_2);
+  myax12_3.SetGoal(cntr_3);
+  myax12_4.SetGoal(cntr_4);
+  myax12_5.SetGoal(cntr_5);
   
-        myled1 = 1;
-        lcd.string(10,10,300,220,YELLOW,FONT3,TRANS,"Before set goal 150");  //write a string on the screen
-        myax12_1.SetGoal(150);    // go to 150 degrees
-        myax12_2.SetGoal(150);    // go to 150 degrees
-        myax12_3.SetGoal(150);    // go to 150 degrees
-        myax12_4.SetGoal(150);    // go to 150 degrees
-        myax12_5.SetGoal(290);    // go to 150 degrees
-        lcd.string(10,70,300,220,YELLOW,FONT3,TRANS,"after goal 150");  //write a string on the screen
-        wait_ms(1000);
-        lcd.erase();
-        myled1 = 0;
-        myled4 = 1;
-        lcd.string(10,10,300,220,YELLOW,FONT3,TRANS,"Before set goal fold up");  //write a string on the screen
-        //myax12_1.SetGoal(300);    // go to 150 degrees
-        myax12_2.SetGoal(225);    // go to 150 degrees
-        myax12_3.SetGoal(0);    // go to 150 degrees
-        myax12_4.SetGoal(90);    // go to 150 degrees
-        myax12_5.SetGoal(250);    // go to 150 degrees
-        lcd.string(10,70,300,220,YELLOW,FONT3,TRANS,"after goal fold up");  //write a string on the screen
-        wait_ms(1000);
-        lcd.erase();
-        myled4 = 0;
+  //start the Robot Control
+  while(1) {
+        //drawing rectangles for manual control and labeling them
+        lcd.drawRectangle(12,13,36,26,MAGENTA,FILL); //x1,y1,x2,y2,color,FILL
+        lcd.drawRectangle(48,13,72,26,MAGENTA,FILL); //x1,y1,x2,y2,color,FILL
+        lcd.string(84,13,300,234,WHITE,FONT4,TRANS,"AX_1");
+        
+        lcd.drawRectangle(12,39,36,52,MAGENTA,FILL); //x1,y1,x2,y2,color,FILL
+        lcd.drawRectangle(48,39,72,52,MAGENTA,FILL); //x1,y1,x2,y2,color,FILL
+        lcd.string(84,39,300,234,WHITE,FONT4,TRANS,"AX_2");
+        
+        lcd.drawRectangle(12,65,36,78,MAGENTA,FILL); //x1,y1,x2,y2,color,FILL
+        lcd.drawRectangle(48,65,72,78,MAGENTA,FILL); //x1,y1,x2,y2,color,FILL
+        lcd.string(84,65,300,234,WHITE,FONT4,TRANS,"AX_3");
+        
+        lcd.drawRectangle(12,91,36,104,MAGENTA,FILL); //x1,y1,x2,y2,color,FILL
+        lcd.drawRectangle(48,91,72,104,MAGENTA,FILL); //x1,y1,x2,y2,color,FILL
+        lcd.string(84,91,300,234,WHITE,FONT4,TRANS,"AX_4");
+        
+        lcd.drawRectangle(12,117,36,130,MAGENTA,FILL); //x1,y1,x2,y2,color,FILL
+        lcd.drawRectangle(48,117,72,130,MAGENTA,FILL); //x1,y1,x2,y2,color,FILL
+        lcd.string(84,117,300,234,WHITE,FONT4,TRANS,"AX_5");
+        
+        if(lcd.touchScreen(touch)){// If we receive a touch on the screen
+            if(touch[XCOORD]>=12&&touch[XCOORD]<=36&&touch[YCOORD]>=13&&touch[YCOORD]<=26){
+                //AX_1 down counter to 0
+                if(cntr_1!=0){cntr_1=cntr_1-3;}
+                myax12_1.SetGoal(cntr_1);    // go to cntr_1 degrees
+            }
+            if(touch[XCOORD]>=48&&touch[XCOORD]<=72&&touch[YCOORD]>=13&&touch[YCOORD]<=26){
+                //AX_1 up counter to 300
+                if(cntr_1!=300){cntr_1=cntr_1+3;}
+                myax12_1.SetGoal(cntr_1);    // go to cntr_1 degrees
+            }
+            if(touch[XCOORD]>=12&&touch[XCOORD]<=36&&touch[YCOORD]>=39&&touch[YCOORD]<=52){
+                //AX_2 down counter to 0
+                if(cntr_2!=0){cntr_2=cntr_2-3;}
+                myax12_2.SetGoal(cntr_2);    // go to cntr_1 degrees
+            }
+            if(touch[XCOORD]>=48&&touch[XCOORD]<=72&&touch[YCOORD]>=39&&touch[YCOORD]<=52){
+                //AX_2 up counter to 300
+                if(cntr_2!=300){cntr_2=cntr_2+3;}
+                myax12_2.SetGoal(cntr_2);    // go to cntr_1 degrees
+            }
+            if(touch[XCOORD]>=12&&touch[XCOORD]<=36&&touch[YCOORD]>=65&&touch[YCOORD]<=78){
+                //AX_1 down counter to 0
+                if(cntr_3!=0){cntr_3=cntr_3-3;}
+                myax12_3.SetGoal(cntr_3);    // go to cntr_1 degrees
+            }
+            if(touch[XCOORD]>=48&&touch[XCOORD]<=72&&touch[YCOORD]>=65&&touch[YCOORD]<=78){
+                //AX_1 up counter to 300
+                if(cntr_3!=300){cntr_3=cntr_3+3;}
+                myax12_3.SetGoal(cntr_3);    // go to cntr_1 degrees
+            }
+            if(touch[XCOORD]>=12&&touch[XCOORD]<=36&&touch[YCOORD]>=91&&touch[YCOORD]<=104){
+                //AX_1 down counter to 0
+                if(cntr_4!=0){cntr_4=cntr_4-3;}
+                myax12_4.SetGoal(cntr_4);    // go to cntr_1 degrees
+            }
+            if(touch[XCOORD]>=48&&touch[XCOORD]<=72&&touch[YCOORD]>=91&&touch[YCOORD]<=104){
+                //AX_1 up counter to 300
+                if(cntr_4!=300){cntr_4=cntr_4+3;}
+                myax12_4.SetGoal(cntr_4);    // go to cntr_1 degrees
+            }
+            if(touch[XCOORD]>=12&&touch[XCOORD]<=36&&touch[YCOORD]>=117&&touch[YCOORD]<=130){
+                //AX_1 down counter to 0
+                if(cntr_5!=0){cntr_5=cntr_5-3;}
+                myax12_5.SetGoal(cntr_5);    // go to cntr_1 degrees
+            }
+            if(touch[XCOORD]>=48&&touch[XCOORD]<=72&&touch[YCOORD]>=117&&touch[YCOORD]<=130){
+                //AX_1 up counter to 300
+                if(cntr_5!=300){cntr_5=cntr_5+3;}
+                myax12_5.SetGoal(cntr_5);    // go to cntr_1 degrees
+            }
+            // touched coordinates
+            //snprintf(buffer, 100, "X location = %d", touch[XCOORD]);
+            //lcd.string(12,70,319,239,WHITE,FONT7,TRANS,buffer);
+            //snprintf(buffer, 100, "Y location = %d", touch[YCOORD]);
+            //lcd.string(12,130,319,239,WHITE,FONT7,TRANS,buffer);
+            //lcd.erase();
+        }
+            //lcd.string(12,70,319,239,WHITE,FONT7,TRANS,"X location =   ");
+            //lcd.string(12,130,319,239,WHITE,FONT7,TRANS,"Y location =   ");
+        //myled1 = 1;
+        //wait_ms(500);
+        //myled1 = 0;
+        //myled4 = 1;
+        //wait_ms(500);
+        //myled4 = 0;
     }//end of main while loop
 }