Marco Newman / Mbed 2 deprecated CartesianRobot_Demo

Demo program for a serial controlled cartesian robot with pushbutton and uLCD interfaces.

Dependencies:   mbed mbed-rtos 4DGL-uLCD-SE CartesianRobot

Documentation: https://github.com/MarcoNewman/MbedCartesianRobot

main.cpp@1:43d89b9cf36a, 2021-12-16 (annotated)

Committer:
marcoanewman
Date:
Thu Dec 16 04:34:03 2021 +0000
Revision:
1:43d89b9cf36a
Parent:
0:1416f2304218 
fixed errors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marcoanewman 0:1416f2304218 1 #include "mbed.h"
marcoanewman 0:1416f2304218 2 #include "rtos.h"
marcoanewman 0:1416f2304218 3 #include "uLCD_4DGL.h"
marcoanewman 0:1416f2304218 4 #include "CartesianRobot.h"
marcoanewman 0:1416f2304218 5
marcoanewman 0:1416f2304218 6 Serial pc(USBTX,USBRX);
marcoanewman 0:1416f2304218 7
marcoanewman 0:1416f2304218 8 DigitalIn enable(p13);
marcoanewman 0:1416f2304218 9 DigitalIn set(p14);
marcoanewman 0:1416f2304218 10 DigitalIn controlX(p15);
marcoanewman 0:1416f2304218 11 DigitalIn controlY(p16);
marcoanewman 0:1416f2304218 12 DigitalIn controlZ(p17);
marcoanewman 0:1416f2304218 13
marcoanewman 0:1416f2304218 14 CartesianRobot robot(p22,p23,p24, p25,p26,p27, p28,p29,p30);
marcoanewman 0:1416f2304218 15
marcoanewman 0:1416f2304218 16 uLCD_4DGL uLCD(p9,p10,p11);
marcoanewman 0:1416f2304218 17 Mutex lcd_mutex;
marcoanewman 0:1416f2304218 18
marcoanewman 0:1416f2304218 19 // manage motor driver enable
marcoanewman 0:1416f2304218 20 void enable_switch(void const *args){
marcoanewman 0:1416f2304218 21 while(1){
marcoanewman 0:1416f2304218 22 if (!enable && robot.enabled())
marcoanewman 0:1416f2304218 23 robot.disable();
marcoanewman 0:1416f2304218 24 else if (enable && !robot.enabled())
marcoanewman 0:1416f2304218 25 robot.enable();
marcoanewman 0:1416f2304218 26 }
marcoanewman 0:1416f2304218 27 }
marcoanewman 0:1416f2304218 28
marcoanewman 0:1416f2304218 29 // robot position monitoring
marcoanewman 0:1416f2304218 30 void position_monitor(void const *args){
marcoanewman 0:1416f2304218 31 int x_pos, y_pos, z_pos;
marcoanewman 0:1416f2304218 32 while(1){
marcoanewman 0:1416f2304218 33 x_pos = robot.getXPosition()/8;
marcoanewman 0:1416f2304218 34 y_pos = robot.getYPosition()/8;
marcoanewman 0:1416f2304218 35 z_pos = robot.getZPosition()/8;
marcoanewman 0:1416f2304218 36 lcd_mutex.lock();
marcoanewman 0:1416f2304218 37 uLCD.locate(0,5);
marcoanewman 0:1416f2304218 38 uLCD.printf(" ");
marcoanewman 0:1416f2304218 39 uLCD.locate(0,5);
marcoanewman 0:1416f2304218 40 uLCD.printf("(%d, %d, %d)", x_pos, y_pos, z_pos);
marcoanewman 0:1416f2304218 41 lcd_mutex.unlock();
marcoanewman 0:1416f2304218 42 Thread::wait(200);
marcoanewman 0:1416f2304218 43 }
marcoanewman 0:1416f2304218 44 }
marcoanewman 0:1416f2304218 45
marcoanewman 0:1416f2304218 46 int main()
marcoanewman 0:1416f2304218 47 {
marcoanewman 0:1416f2304218 48 // configure PB and switch internal PullDown
marcoanewman 0:1416f2304218 49 enable.mode(PullDown);
marcoanewman 0:1416f2304218 50 set.mode(PullDown);
marcoanewman 0:1416f2304218 51 controlX.mode(PullDown);
marcoanewman 0:1416f2304218 52 controlY.mode(PullDown);
marcoanewman 0:1416f2304218 53 controlZ.mode(PullDown);
marcoanewman 0:1416f2304218 54 wait(1);
marcoanewman 0:1416f2304218 55
marcoanewman 0:1416f2304218 56 // set robot speeds, accelerations, and decelerations
marcoanewman 0:1416f2304218 57 robot.setXSpeed(1200);
marcoanewman 0:1416f2304218 58 robot.setYSpeed(1200);
marcoanewman 0:1416f2304218 59 robot.setZSpeed(2400);
marcoanewman 1:43d89b9cf36a 60 robot.setXAcc(0);
marcoanewman 1:43d89b9cf36a 61 robot.setYAcc(0);
marcoanewman 1:43d89b9cf36a 62 robot.setZAcc(0);
marcoanewman 1:43d89b9cf36a 63 robot.setXDec(0);
marcoanewman 1:43d89b9cf36a 64 robot.setYDec(0);
marcoanewman 1:43d89b9cf36a 65 robot.setZDec(0);
marcoanewman 0:1416f2304218 66
marcoanewman 0:1416f2304218 67 // baud rate to max for fast display
marcoanewman 0:1416f2304218 68 uLCD.baudrate(3000000);
marcoanewman 0:1416f2304218 69 uLCD.display_control(PORTRAIT);
marcoanewman 0:1416f2304218 70
marcoanewman 0:1416f2304218 71 // start robot, enable, and position monitoring threads
marcoanewman 0:1416f2304218 72 robot.startManager();
marcoanewman 1:43d89b9cf36a 73 Thread t1(enable_switch);
marcoanewman 1:43d89b9cf36a 74 Thread t2(position_monitor);
marcoanewman 0:1416f2304218 75
marcoanewman 0:1416f2304218 76 // set the origin position
marcoanewman 0:1416f2304218 77 lcd_mutex.lock();
marcoanewman 0:1416f2304218 78 uLCD.locate(0,0);
marcoanewman 0:1416f2304218 79 uLCD.printf("Press set when at desired 0 pos...");
marcoanewman 0:1416f2304218 80 while(!set);
marcoanewman 0:1416f2304218 81 robot.setOrigin();
marcoanewman 0:1416f2304218 82 uLCD.cls();
marcoanewman 0:1416f2304218 83
marcoanewman 0:1416f2304218 84 // set upper limits
marcoanewman 0:1416f2304218 85 uLCD.printf("Press set when at desired max pos...");
marcoanewman 0:1416f2304218 86 lcd_mutex.unlock();
marcoanewman 0:1416f2304218 87 while(!set){
marcoanewman 0:1416f2304218 88 if (robot.stopped() && controlX)
marcoanewman 0:1416f2304218 89 robot.moveX(80);
marcoanewman 0:1416f2304218 90 if (robot.stopped() && controlY)
marcoanewman 0:1416f2304218 91 robot.moveY(80);
marcoanewman 0:1416f2304218 92 if (robot.stopped() && controlZ)
marcoanewman 0:1416f2304218 93 robot.moveZ(80);
marcoanewman 0:1416f2304218 94 }
marcoanewman 0:1416f2304218 95 robot.setLimits();
marcoanewman 0:1416f2304218 96
marcoanewman 0:1416f2304218 97 // prepare for motion control
marcoanewman 0:1416f2304218 98 lcd_mutex.lock();
marcoanewman 0:1416f2304218 99 uLCD.cls();
marcoanewman 0:1416f2304218 100 uLCD.printf("Enter Positions...\nFormat:\nx,y,z");
marcoanewman 0:1416f2304218 101 uLCD.locate(0,11);
marcoanewman 0:1416f2304218 102 uLCD.printf("Limits:\n(%d, %d, %d)",
marcoanewman 0:1416f2304218 103 robot.getXLimit()/8, robot.getYLimit()/8, robot.getZLimit()/8
marcoanewman 0:1416f2304218 104 );
marcoanewman 0:1416f2304218 105 lcd_mutex.unlock();
marcoanewman 0:1416f2304218 106
marcoanewman 0:1416f2304218 107 // buffer movements as default speed
marcoanewman 0:1416f2304218 108 int x, y, z;
marcoanewman 0:1416f2304218 109 int x_s = 600;
marcoanewman 0:1416f2304218 110 int y_s = 600;
marcoanewman 0:1416f2304218 111 int z_s = 1000;
marcoanewman 0:1416f2304218 112 while(1){
marcoanewman 0:1416f2304218 113 if (pc.readable()){
marcoanewman 0:1416f2304218 114 lcd_mutex.lock();
marcoanewman 0:1416f2304218 115 pc.scanf("%d,%d,%d", &x, &y, &z);
marcoanewman 0:1416f2304218 116 pc.printf("Position Buffered\n");
marcoanewman 0:1416f2304218 117 pc.printf("x=%d, y=%d, z=%d\n", x, y, z);
marcoanewman 0:1416f2304218 118 lcd_mutex.unlock();
marcoanewman 0:1416f2304218 119 robot.moveToXYZ(x*8,y*8,z*8,(float)x_s,(float)y_s,(float)z_s);
marcoanewman 0:1416f2304218 120 }
marcoanewman 0:1416f2304218 121 }
marcoanewman 0:1416f2304218 122 }