Final code for our 4180 Drawing Robot!
Dependencies: 4DGL-uLCD-SE gCodeParser mbed
main.cpp@2:ba15545a4ccf, 2014-04-30 (annotated)
- Committer:
- jford38
- Date:
- Wed Apr 30 16:40:10 2014 +0000
- Revision:
- 2:ba15545a4ccf
- Parent:
- 1:ad895d72e9ed
Added Visual Studio Source files and README.; Alejandro.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jford38 | 0:40576dfac535 | 1 | #include "mbed.h" |
jford38 | 0:40576dfac535 | 2 | #include "gparser.h" |
jford38 | 0:40576dfac535 | 3 | #include "MODSERIAL.h" |
jford38 | 0:40576dfac535 | 4 | #include "motor.h" |
jford38 | 0:40576dfac535 | 5 | #include "drawBot.h" |
jford38 | 0:40576dfac535 | 6 | #include "uLCD_4DGL.h" |
jford38 | 1:ad895d72e9ed | 7 | #include <ctype.h> |
jford38 | 0:40576dfac535 | 8 | |
jford38 | 0:40576dfac535 | 9 | MODSERIAL gpc(USBTX, USBRX); |
jford38 | 0:40576dfac535 | 10 | uLCD_4DGL uLCD(p9, p10, p11); |
jford38 | 0:40576dfac535 | 11 | |
jford38 | 1:ad895d72e9ed | 12 | G_cmd gcmd_list[CMD_LIST_SIZE]; // buffer to be filled with commands, main program retrieves commands from here |
jford38 | 1:ad895d72e9ed | 13 | |
jford38 | 1:ad895d72e9ed | 14 | extern int list_position; |
jford38 | 0:40576dfac535 | 15 | |
jford38 | 0:40576dfac535 | 16 | Motor mL(p29, p30, p26, p27, p28, p25, RIGHT_MOTOR); |
jford38 | 0:40576dfac535 | 17 | Motor mR(p12, p13, p16, p15, p14, p17, LEFT_MOTOR); |
jford38 | 0:40576dfac535 | 18 | DrawBot bot(&mL, &mR, p21, 0.5, 45.125); |
jford38 | 0:40576dfac535 | 19 | |
jford38 | 0:40576dfac535 | 20 | int main() { |
jford38 | 0:40576dfac535 | 21 | |
jford38 | 0:40576dfac535 | 22 | parserInit(); |
jford38 | 0:40576dfac535 | 23 | |
jford38 | 1:ad895d72e9ed | 24 | fillInCmdList(); |
jford38 | 1:ad895d72e9ed | 25 | uLCD.printf("Commands copied %d\n", list_position); |
jford38 | 0:40576dfac535 | 26 | |
jford38 | 1:ad895d72e9ed | 27 | for(int i=0; i<list_position; i++) { |
jford38 | 1:ad895d72e9ed | 28 | //uLCD.printf("G:%d X:%.2f Y:%.2f Z:%.2f I:%.2f J:%.2f F:%.2f\r\n", gcmd_list[i].G, gcmd_list[i].X, gcmd_list[i].Y, gcmd_list[i].Z, gcmd_list[i].I, gcmd_list[i].J, gcmd_list[i].F); |
jford38 | 1:ad895d72e9ed | 29 | //wait(10); |
jford38 | 1:ad895d72e9ed | 30 | //uLCD.cls(); |
jford38 | 1:ad895d72e9ed | 31 | switch(gcmd_list[i].G) { |
jford38 | 0:40576dfac535 | 32 | case 0: |
jford38 | 0:40576dfac535 | 33 | case 1: |
jford38 | 0:40576dfac535 | 34 | //Draw a line. |
jford38 | 1:ad895d72e9ed | 35 | if(gcmd_list[i].X != -1 && gcmd_list[i].Y != -1) { |
jford38 | 1:ad895d72e9ed | 36 | //uLCD.printf("Going to: %.2f, %.2f\n",gcmd_list[i].X,gcmd_list[i].Y); |
jford38 | 1:ad895d72e9ed | 37 | bot.line_safe(gcmd_list[i].X, -1*gcmd_list[i].Y+20); |
jford38 | 0:40576dfac535 | 38 | } |
jford38 | 1:ad895d72e9ed | 39 | if(gcmd_list[i].Z > 0) { |
jford38 | 1:ad895d72e9ed | 40 | //uLCD.printf("PEN UP\n"); |
jford38 | 0:40576dfac535 | 41 | bot.pen_up(); |
jford38 | 1:ad895d72e9ed | 42 | }else if(gcmd_list[i].Z < 0) { |
jford38 | 1:ad895d72e9ed | 43 | //uLCD.printf("PEN DOWN\n"); |
jford38 | 0:40576dfac535 | 44 | bot.pen_down(); |
jford38 | 0:40576dfac535 | 45 | } |
jford38 | 0:40576dfac535 | 46 | break; |
jford38 | 0:40576dfac535 | 47 | case 2: |
jford38 | 0:40576dfac535 | 48 | //Draw an arc or something. |
jford38 | 1:ad895d72e9ed | 49 | bot.arc(gcmd_list[i].I, gcmd_list[i].J, gcmd_list[i].X, gcmd_list[i].Y, CCW_ARC); |
jford38 | 0:40576dfac535 | 50 | break; |
jford38 | 0:40576dfac535 | 51 | case 3: |
jford38 | 0:40576dfac535 | 52 | //Draw a different arc or something. |
jford38 | 1:ad895d72e9ed | 53 | bot.arc(gcmd_list[i].I, gcmd_list[i].J, gcmd_list[i].X, gcmd_list[i].Y, CW_ARC); |
jford38 | 0:40576dfac535 | 54 | break; |
jford38 | 0:40576dfac535 | 55 | default: |
jford38 | 0:40576dfac535 | 56 | break; |
jford38 | 0:40576dfac535 | 57 | |
jford38 | 0:40576dfac535 | 58 | } |
jford38 | 0:40576dfac535 | 59 | } |
jford38 | 0:40576dfac535 | 60 | bot.disable(); |
jford38 | 1:ad895d72e9ed | 61 | } |
jford38 | 1:ad895d72e9ed | 62 | |
jford38 | 1:ad895d72e9ed | 63 | //=================================================== |
jford38 | 1:ad895d72e9ed | 64 | /* |
jford38 | 1:ad895d72e9ed | 65 | * mbed GCode parsing Library |
jford38 | 1:ad895d72e9ed | 66 | * |
jford38 | 1:ad895d72e9ed | 67 | */ |
jford38 | 1:ad895d72e9ed | 68 | |
jford38 | 1:ad895d72e9ed | 69 | |
jford38 | 1:ad895d72e9ed | 70 | |
jford38 | 1:ad895d72e9ed | 71 | #define FEEDME 10 |
jford38 | 1:ad895d72e9ed | 72 | #define END_OF_TRANSMISSION 23 |
jford38 | 1:ad895d72e9ed | 73 | //extern MODSERIAL gpc; |
jford38 | 1:ad895d72e9ed | 74 | |
jford38 | 1:ad895d72e9ed | 75 | /************VARS*******************************/ |
jford38 | 1:ad895d72e9ed | 76 | DigitalOut led1(LED1); |
jford38 | 1:ad895d72e9ed | 77 | DigitalOut led2(LED2); |
jford38 | 1:ad895d72e9ed | 78 | DigitalOut led3(LED3); |
jford38 | 1:ad895d72e9ed | 79 | DigitalOut led4(LED4); |
jford38 | 1:ad895d72e9ed | 80 | |
jford38 | 1:ad895d72e9ed | 81 | // buffer used to transfer the serial buffer contents and parse them into cmd structure |
jford38 | 1:ad895d72e9ed | 82 | char rx_buff[CMD_BUFFER_SIZE]; |
jford38 | 1:ad895d72e9ed | 83 | |
jford38 | 1:ad895d72e9ed | 84 | volatile bool received; // indicates when a new cmd is available in the serial buffer |
jford38 | 1:ad895d72e9ed | 85 | bool endTransmission; // indicates that all the commands have been sent from the pc |
jford38 | 1:ad895d72e9ed | 86 | |
jford38 | 1:ad895d72e9ed | 87 | int list_position; // counter to index the array of command structures |
jford38 | 1:ad895d72e9ed | 88 | |
jford38 | 1:ad895d72e9ed | 89 | |
jford38 | 1:ad895d72e9ed | 90 | /* This initializes the serial port params to match the PC application |
jford38 | 1:ad895d72e9ed | 91 | * the interrupt handlers are also initialized here |
jford38 | 1:ad895d72e9ed | 92 | */ |
jford38 | 1:ad895d72e9ed | 93 | void parserInit() |
jford38 | 1:ad895d72e9ed | 94 | { |
jford38 | 1:ad895d72e9ed | 95 | list_position = 0; |
jford38 | 1:ad895d72e9ed | 96 | received = false; |
jford38 | 1:ad895d72e9ed | 97 | endTransmission = false; |
jford38 | 1:ad895d72e9ed | 98 | gpc.baud(921600); |
jford38 | 1:ad895d72e9ed | 99 | gpc.autoDetectChar('\0'); //<--to catch the null terminator at the end |
jford38 | 1:ad895d72e9ed | 100 | gpc.attach(&cmd_Received, MODSERIAL::RxAutoDetect); // declares callback for when cmd is ready |
jford38 | 1:ad895d72e9ed | 101 | gpc.rxBufferFlush(); // flush the buffer just in case |
jford38 | 1:ad895d72e9ed | 102 | } |
jford38 | 1:ad895d72e9ed | 103 | |
jford38 | 1:ad895d72e9ed | 104 | /* Parses the received messages into a G_cmd structure |
jford38 | 1:ad895d72e9ed | 105 | * it waits for the serial buffer to contain an entire command, |
jford38 | 1:ad895d72e9ed | 106 | * parses the gcode and sends the FEEDME command |
jford38 | 1:ad895d72e9ed | 107 | * to the pc to request another gcode line |
jford38 | 1:ad895d72e9ed | 108 | * returns a pointer to the allocated list of commands. |
jford38 | 1:ad895d72e9ed | 109 | */ |
jford38 | 1:ad895d72e9ed | 110 | int fillInCmdList() |
jford38 | 1:ad895d72e9ed | 111 | { |
jford38 | 1:ad895d72e9ed | 112 | // end_flag_received should be detected in parseGcode() |
jford38 | 1:ad895d72e9ed | 113 | while(endTransmission == false) { |
jford38 | 1:ad895d72e9ed | 114 | // Light up LED1 to indicate the mbed is ready to receive Serial commands |
jford38 | 1:ad895d72e9ed | 115 | led1 = 1; |
jford38 | 1:ad895d72e9ed | 116 | if(received == true) { //if the buffer has received the null terminator |
jford38 | 1:ad895d72e9ed | 117 | |
jford38 | 1:ad895d72e9ed | 118 | // Signal that we are now parsing by lighting LED2 |
jford38 | 1:ad895d72e9ed | 119 | //led1 = 0; |
jford38 | 1:ad895d72e9ed | 120 | led2 = 1; |
jford38 | 1:ad895d72e9ed | 121 | parseGcode(); |
jford38 | 1:ad895d72e9ed | 122 | gpc.rxBufferFlush(); // flushing the buffer just in case |
jford38 | 1:ad895d72e9ed | 123 | received = false; |
jford38 | 1:ad895d72e9ed | 124 | gpc.putc(FEEDME); // requesting next command from the pc |
jford38 | 1:ad895d72e9ed | 125 | led2 = 0; // parsing stage is over |
jford38 | 1:ad895d72e9ed | 126 | } |
jford38 | 1:ad895d72e9ed | 127 | } |
jford38 | 1:ad895d72e9ed | 128 | |
jford38 | 1:ad895d72e9ed | 129 | //uLCD.printf("Commands copied %d\n", list_position); |
jford38 | 1:ad895d72e9ed | 130 | //for(int i = 0; i < list_position; i++) { |
jford38 | 1:ad895d72e9ed | 131 | // uLCD.printf("G:%d X:%.2f Y:%.2f Z:%.2f I:%.2f J:%.2f F:%.2f\r\n", gcmd_list[i].G, gcmd_list[i].X, gcmd_list[i].Y, gcmd_list[i].Z, gcmd_list[i].I, gcmd_list[i].J, gcmd_list[i].F); |
jford38 | 1:ad895d72e9ed | 132 | //wait(10); |
jford38 | 1:ad895d72e9ed | 133 | //uLCD.cls(); |
jford38 | 1:ad895d72e9ed | 134 | //} |
jford38 | 1:ad895d72e9ed | 135 | |
jford38 | 1:ad895d72e9ed | 136 | led1 = 1; |
jford38 | 1:ad895d72e9ed | 137 | wait(0.5); |
jford38 | 1:ad895d72e9ed | 138 | led1 = 0; |
jford38 | 1:ad895d72e9ed | 139 | led2 = 1; |
jford38 | 1:ad895d72e9ed | 140 | wait(0.5); |
jford38 | 1:ad895d72e9ed | 141 | led2 = 0; |
jford38 | 1:ad895d72e9ed | 142 | led3 = 1; |
jford38 | 1:ad895d72e9ed | 143 | wait(0.5); |
jford38 | 1:ad895d72e9ed | 144 | led3 = 0; |
jford38 | 1:ad895d72e9ed | 145 | led1 = 1; |
jford38 | 1:ad895d72e9ed | 146 | wait(0.5); |
jford38 | 1:ad895d72e9ed | 147 | led3 = 1; |
jford38 | 1:ad895d72e9ed | 148 | led2 = 1; |
jford38 | 1:ad895d72e9ed | 149 | return (list_position); |
jford38 | 1:ad895d72e9ed | 150 | } |
jford38 | 1:ad895d72e9ed | 151 | |
jford38 | 1:ad895d72e9ed | 152 | //interrupt routine called when the null terminator is received |
jford38 | 1:ad895d72e9ed | 153 | void cmd_Received(MODSERIAL_IRQ_INFO *q) |
jford38 | 1:ad895d72e9ed | 154 | { |
jford38 | 1:ad895d72e9ed | 155 | MODSERIAL *sys = q->serial; |
jford38 | 1:ad895d72e9ed | 156 | sys->move(rx_buff, CMD_BUFFER_SIZE);; |
jford38 | 1:ad895d72e9ed | 157 | received = true; |
jford38 | 1:ad895d72e9ed | 158 | } |
jford38 | 1:ad895d72e9ed | 159 | //TODO: Detect the connection finish characters and set a flag so fillInCmdBuff can return |
jford38 | 1:ad895d72e9ed | 160 | // Parses the received message and populates the structure |
jford38 | 1:ad895d72e9ed | 161 | void parseGcode() |
jford38 | 1:ad895d72e9ed | 162 | { |
jford38 | 1:ad895d72e9ed | 163 | // checking for the termination of connection |
jford38 | 1:ad895d72e9ed | 164 | if(rx_buff[0] == END_OF_TRANSMISSION) { |
jford38 | 1:ad895d72e9ed | 165 | led4 = 1; //received end of transmission from pc |
jford38 | 1:ad895d72e9ed | 166 | endTransmission = true; |
jford38 | 1:ad895d72e9ed | 167 | return; |
jford38 | 1:ad895d72e9ed | 168 | } |
jford38 | 1:ad895d72e9ed | 169 | |
jford38 | 1:ad895d72e9ed | 170 | // in case there are leading spaces |
jford38 | 1:ad895d72e9ed | 171 | char* cmdP = strtok(rx_buff, " G"); |
jford38 | 1:ad895d72e9ed | 172 | |
jford38 | 1:ad895d72e9ed | 173 | char coord_label[2]; |
jford38 | 1:ad895d72e9ed | 174 | |
jford38 | 1:ad895d72e9ed | 175 | //int g = atoi(cmdP); |
jford38 | 1:ad895d72e9ed | 176 | |
jford38 | 1:ad895d72e9ed | 177 | //uLCD.printf("list pos is: %d\n", list_position); |
jford38 | 1:ad895d72e9ed | 178 | |
jford38 | 1:ad895d72e9ed | 179 | //fill out the command number field |
jford38 | 1:ad895d72e9ed | 180 | gcmd_list[list_position].G = atoi(cmdP); |
jford38 | 1:ad895d72e9ed | 181 | gcmd_list[list_position].X = -1; |
jford38 | 1:ad895d72e9ed | 182 | gcmd_list[list_position].Y = -1; |
jford38 | 1:ad895d72e9ed | 183 | gcmd_list[list_position].Z = 0; |
jford38 | 1:ad895d72e9ed | 184 | gcmd_list[list_position].F = -1; |
jford38 | 1:ad895d72e9ed | 185 | gcmd_list[list_position].I = -1; |
jford38 | 1:ad895d72e9ed | 186 | gcmd_list[list_position].J = -1; |
jford38 | 1:ad895d72e9ed | 187 | |
jford38 | 1:ad895d72e9ed | 188 | //uLCD.printf("%d: ", gcmd_list[list_position].G); |
jford38 | 1:ad895d72e9ed | 189 | |
jford38 | 1:ad895d72e9ed | 190 | // Looping to get the arguments |
jford38 | 1:ad895d72e9ed | 191 | while (cmdP != NULL) { |
jford38 | 1:ad895d72e9ed | 192 | // Retrieve the next Label |
jford38 | 1:ad895d72e9ed | 193 | cmdP = strtok (NULL, " G"); |
jford38 | 1:ad895d72e9ed | 194 | // cmdP should be pointing to a letter now |
jford38 | 1:ad895d72e9ed | 195 | //uLCD.printf("Coord: %s.\n", cmdP); |
jford38 | 1:ad895d72e9ed | 196 | coord_label[0] = cmdP[0]; |
jford38 | 1:ad895d72e9ed | 197 | coord_label[1] = cmdP[1]; |
jford38 | 1:ad895d72e9ed | 198 | |
jford38 | 1:ad895d72e9ed | 199 | // retrieve the number after the letter |
jford38 | 1:ad895d72e9ed | 200 | cmdP = strtok (NULL, " G"); |
jford38 | 1:ad895d72e9ed | 201 | |
jford38 | 1:ad895d72e9ed | 202 | // now print the number |
jford38 | 1:ad895d72e9ed | 203 | //uLCD.printf("%s ", cmdP); |
jford38 | 1:ad895d72e9ed | 204 | /* |
jford38 | 1:ad895d72e9ed | 205 | gcmd_list[list_position].X = -1; |
jford38 | 1:ad895d72e9ed | 206 | gcmd_list[list_position].Y = -1; |
jford38 | 1:ad895d72e9ed | 207 | gcmd_list[list_position].Z = -2; |
jford38 | 1:ad895d72e9ed | 208 | gcmd_list[list_position].F = -1; |
jford38 | 1:ad895d72e9ed | 209 | gcmd_list[list_position].I = -1; |
jford38 | 1:ad895d72e9ed | 210 | gcmd_list[list_position].J = -1; |
jford38 | 1:ad895d72e9ed | 211 | */ |
jford38 | 1:ad895d72e9ed | 212 | switch(coord_label[0]) { |
jford38 | 1:ad895d72e9ed | 213 | case 'X': |
jford38 | 1:ad895d72e9ed | 214 | gcmd_list[list_position].X = atof(cmdP); |
jford38 | 1:ad895d72e9ed | 215 | break; |
jford38 | 1:ad895d72e9ed | 216 | case 'Y': |
jford38 | 1:ad895d72e9ed | 217 | gcmd_list[list_position].Y = atof(cmdP); |
jford38 | 1:ad895d72e9ed | 218 | break; |
jford38 | 1:ad895d72e9ed | 219 | case 'Z': |
jford38 | 1:ad895d72e9ed | 220 | gcmd_list[list_position].Z = atof(cmdP); |
jford38 | 1:ad895d72e9ed | 221 | break; |
jford38 | 1:ad895d72e9ed | 222 | case 'F': |
jford38 | 1:ad895d72e9ed | 223 | gcmd_list[list_position].F = atof(cmdP); |
jford38 | 1:ad895d72e9ed | 224 | break; |
jford38 | 1:ad895d72e9ed | 225 | case 'I': |
jford38 | 1:ad895d72e9ed | 226 | gcmd_list[list_position].I = atof(cmdP); |
jford38 | 1:ad895d72e9ed | 227 | break; |
jford38 | 1:ad895d72e9ed | 228 | case 'J': |
jford38 | 1:ad895d72e9ed | 229 | gcmd_list[list_position].J = atof(cmdP); |
jford38 | 1:ad895d72e9ed | 230 | break; |
jford38 | 1:ad895d72e9ed | 231 | default: |
jford38 | 1:ad895d72e9ed | 232 | |
jford38 | 1:ad895d72e9ed | 233 | break; |
jford38 | 1:ad895d72e9ed | 234 | }// switch |
jford38 | 1:ad895d72e9ed | 235 | } // while |
jford38 | 1:ad895d72e9ed | 236 | list_position++; |
jford38 | 1:ad895d72e9ed | 237 | } |