main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "uLCD_4DGL.h"
00004 #include "CartesianRobot.h"
00005 
00006 Serial pc(USBTX,USBRX);
00007 
00008 DigitalIn enable(p13);
00009 DigitalIn set(p14);
00010 DigitalIn controlX(p15);
00011 DigitalIn controlY(p16);
00012 DigitalIn controlZ(p17);
00013 
00014 CartesianRobot robot(p22,p23,p24, p25,p26,p27, p28,p29,p30);
00015 
00016 uLCD_4DGL uLCD(p9,p10,p11);
00017 Mutex lcd_mutex;
00018 
00019 // manage motor driver enable
00020 void enable_switch(void const *args){
00021     while(1){
00022         if (!enable && robot.enabled())
00023             robot.disable();
00024         else if (enable && !robot.enabled())
00025             robot.enable();
00026     }
00027 }
00028 
00029 // robot position monitoring
00030 void position_monitor(void const *args){
00031     int x_pos, y_pos, z_pos;
00032     while(1){
00033         x_pos = robot.getXPosition()/8;
00034         y_pos = robot.getYPosition()/8;
00035         z_pos = robot.getZPosition()/8;
00036         lcd_mutex.lock();
00037         uLCD.locate(0,5);
00038         uLCD.printf("                 ");
00039         uLCD.locate(0,5);
00040         uLCD.printf("(%d, %d, %d)", x_pos, y_pos, z_pos);  
00041         lcd_mutex.unlock();
00042         Thread::wait(200);
00043     }
00044 } 
00045 
00046 int main()
00047 {
00048     // configure PB and switch internal PullDown
00049     enable.mode(PullDown);
00050     set.mode(PullDown);
00051     controlX.mode(PullDown);
00052     controlY.mode(PullDown);
00053     controlZ.mode(PullDown);
00054     wait(1);
00055     
00056     // set robot speeds, accelerations, and decelerations
00057     robot.setXSpeed(1200);
00058     robot.setYSpeed(1200);
00059     robot.setZSpeed(2400);
00060     robot.setXAcc(0);
00061     robot.setYAcc(0);
00062     robot.setZAcc(0);
00063     robot.setXDec(0);
00064     robot.setYDec(0);
00065     robot.setZDec(0);
00066     
00067     // baud rate to max for fast display
00068     uLCD.baudrate(3000000);
00069     uLCD.display_control(PORTRAIT);
00070     
00071     // start robot, enable, and position monitoring threads
00072     robot.startManager();
00073     Thread t1(enable_switch);
00074     Thread t2(position_monitor);
00075     
00076     // set the origin position
00077     lcd_mutex.lock();
00078     uLCD.locate(0,0);
00079     uLCD.printf("Press set when at desired 0 pos...");
00080     while(!set);
00081     robot.setOrigin();
00082     uLCD.cls();
00083     
00084     // set upper limits
00085     uLCD.printf("Press set when at desired max pos...");
00086     lcd_mutex.unlock();
00087     while(!set){
00088         if (robot.stopped() && controlX)
00089             robot.moveX(80);
00090         if (robot.stopped() && controlY)
00091             robot.moveY(80);
00092         if (robot.stopped() && controlZ)
00093             robot.moveZ(80);    
00094     }
00095     robot.setLimits();
00096     
00097     // prepare for motion control
00098     lcd_mutex.lock();
00099     uLCD.cls();
00100     uLCD.printf("Enter Positions...\nFormat:\nx,y,z");
00101     uLCD.locate(0,11);
00102     uLCD.printf("Limits:\n(%d, %d, %d)", 
00103         robot.getXLimit()/8, robot.getYLimit()/8, robot.getZLimit()/8
00104     );
00105     lcd_mutex.unlock();
00106     
00107     // buffer movements as default speed
00108     int x, y, z;
00109     int x_s = 600;
00110     int y_s = 600;
00111     int z_s = 1000;
00112     while(1){
00113         if (pc.readable()){
00114             lcd_mutex.lock();
00115             pc.scanf("%d,%d,%d", &x, &y, &z);
00116             pc.printf("Position Buffered\n");
00117             pc.printf("x=%d, y=%d, z=%d\n", x, y, z);
00118             lcd_mutex.unlock();
00119             robot.moveToXYZ(x*8,y*8,z*8,(float)x_s,(float)y_s,(float)z_s);
00120         }
00121     }
00122 }