Aimen Al-Refai
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/
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 |
diff -r de8b38a39938 -r 41b605f9cdd5 main.cpp
--- 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
}