feederController.cpp

00001 #include "mbed.h"
00002 #include "USBSerial.h"
00003 #include "HBridge.h"
00004 #include "feeder.h"
00005 #include "IAP_LPC11U.h"
00006 
00007 extern "C" void mbed_mac_address(char *);
00008 
00009 /************
00010 TODO LIST:
00011 *************
00012 * printf needs to be replace with a function that putc instead for non-formatted strings
00013 * Convert lanes to a class so they there isn't duplicate code
00014 * Store lane distance in EEPROM
00015 * Store retrievable aribitrary text for the lane in EEPROM (loaded part, openpnp part feeder name, whatever)
00016 *************/
00017 
00018 
00019 // SETTINGS
00020 #define TAPE_MAX_PULL_TIME 3 // seconds
00021 #define TAPE_MIN_PULL_TIME 1 // seconds
00022 
00023 #define FEED_FWD_ENC_PULSE 2 // it's actually this value + 1
00024 // encoder pulses: >(lane0FeedDistance * FEED_FWD_ENC_PULSE)
00025 // 3 for 1 position, 5 for 2 positions
00026 
00027 
00028 /*****************************************************************************
00029 **** GLOBALS
00030 *****************************************************************************/
00031 
00032 // SERIAL COMMAND INPUT
00033 enum command_t { READYFORCOMMAND, FEED, PICK, CLOSE, CONFIG };
00034 command_t pcCommand = READYFORCOMMAND;
00035 int pcCommandPos = 0;
00036 
00037 enum config_t { NONE, LANE };
00038 config_t pcConfig = NONE;
00039 
00040 enum lane_t { UNSELECTED, ZERO, ONE };
00041 lane_t selectedLane = UNSELECTED;
00042 
00043 // LANE STATES
00044 enum laneState_t { IDLE, PICKING, FEEDING, REVERSING };
00045 
00046 laneState_t lane0State = IDLE;
00047 laneState_t lane1State = IDLE;
00048 
00049 // LANE EDGE STATES
00050 int lane0LastEdge = 0;
00051 int lane1LastEdge = 0;
00052 
00053 // LANE EDGE COUNTERS
00054 int lane0FeedEdge = 0;
00055 int lane1FeedEdge = 0;
00056 
00057 // NUMBER OF POSITIONS TO FEED THROUGH
00058 int lane0FeedDistance = 1; // 1 = 2mm, 2 = 4mm
00059 int lane1FeedDistance = 1;
00060 
00061 // FEEDER END BUTTON STATES
00062 int button0LastState = 0;
00063 int button1LastState = 0;
00064 
00065 // FEED STATE
00066 bool lane0Feed = 0;
00067 bool lane1Feed = 0;
00068 
00069 // PICKUP STATE
00070 bool tape0Takeup = 0;
00071 bool tape1Takeup = 0;
00072 
00073 // FEED STOPPER
00074 Timeout lane0Feedout;
00075 Timeout lane1Feedout;
00076 
00077 // TAPE PICKUP STOPPER
00078 Timer tape0PullTime;
00079 Timer tape1PullTime;
00080 
00081 /*****************************************************************************
00082 **** HARDWARE
00083 *****************************************************************************/
00084 
00085 // SOLENOIDS (TAPE COVER) - keep above usb to turn off quickly
00086 DigitalOut SOLENOID_0(PIN_SOLCTL_0, 0);
00087 DigitalOut SOLENOID_1(PIN_SOLCTL_1, 0);
00088 
00089 // USB VCOM UART for MBED Serial Port Driver
00090 USBSerial pc(0x1f00, 0x2012, 0x0001, false);
00091 
00092 // RS485
00093 //Serial control(PIN_UART_TXD, PIN_UART_RXD, 115200);
00094 DigitalOut RS485_DIR(PIN_RS485_DIR);
00095 
00096 // MOTOR DRIVERS - FEED
00097 HBridge feedMotor1(PIN_H1_IN1,PIN_H1_IN2);
00098 HBridge feedMotor0(PIN_H1_IN3,PIN_H1_IN4);
00099 
00100 // MOTOR DRIVERS - TAPE
00101 HBridge tapeMotor0(PIN_H2_IN2, PIN_H2_IN1);
00102 HBridge tapeMotor1(PIN_H2_IN4, PIN_H2_IN3);
00103 
00104 // GATES
00105 InterruptIn LN0_FEEDGATE(PIN_GATE_0);
00106 InterruptIn LN1_FEEDGATE(PIN_GATE_1);
00107 InterruptIn LN0_TAPEGATE(PIN_GATE_2);
00108 InterruptIn LN1_TAPEGATE(PIN_GATE_3);
00109 
00110 // FEEDER END BUTTONS
00111 InterruptIn BUTTON_GREEN_0(PIN_BTN_GREEN_0);
00112 InterruptIn BUTTON_GREEN_1(PIN_BTN_GREEN_1);
00113 
00114 InterruptIn BUTTON_YELLOW_0(PIN_LED_YELLOW_0);
00115 InterruptIn BUTTON_YELLOW_1(PIN_LED_YELLOW_1);
00116 
00117 InterruptIn BUTTON_BOARD_FEED(PIN_BOARD_FEED);
00118 
00119 // FEEDER END LEDS
00120 DigitalOut LED_0(PIN_BTN_LED_0);
00121 DigitalOut LED_1(PIN_BTN_LED_1);
00122 
00123 // DIGITAL CONTROL INPUT
00124 InterruptIn LANEIN_0(PIN_LANECTL_0);
00125 InterruptIn LANEIN_1(PIN_LANECTL_1);
00126 
00127 
00128 
00129 /*****************************************************************************
00130 **** TAPE SLACK PICKUP
00131 *****************************************************************************/
00132 
00133 void stopTapeL0() 
00134 { 
00135     tape0PullTime.stop();
00136     tape0Takeup = 0; 
00137     tapeMotor0.Coast(); 
00138 }
00139 
00140 
00141 void stopTapeL1() 
00142 { 
00143     tape1PullTime.stop();
00144     tape1Takeup = 0; 
00145     tapeMotor1.Coast(); 
00146 }
00147 
00148 
00149 /*****************************************************************************
00150 **** BUTTON INTERRUPTS
00151 *****************************************************************************/
00152 
00153 void button_yellow_0_int()
00154 {
00155     pc.printf("Yellow Button 0 Press\r\n");
00156 }
00157 
00158 void button_yellow_1_int()
00159 {
00160     pc.printf("Yellow Button 1 Press\r\n");
00161 }
00162 
00163 
00164 /*****************************************************************************
00165 **** LANE 0 FEED CONTROL
00166 *****************************************************************************/
00167 
00168 void setLane0Picking()
00169 {
00170     lane0State = PICKING;
00171     
00172     //todo: PwmOut
00173     SOLENOID_0 = 1;
00174 }
00175 
00176 void setLane0Feeding()
00177 {
00178     lane0State = FEEDING;
00179     lane0FeedEdge = 0;
00180     lane0LastEdge = LN0_FEEDGATE;
00181     
00182     // solenoid release
00183     SOLENOID_0 = 0;
00184     
00185     lane0Feed = 1;
00186     // motor on
00187     feedMotor0.Forward();
00188 }
00189 
00190 void setLane0Reversing()
00191 {
00192     feedMotor0.Brake(); // yes..  this is only going to brake for a few dozen uS...
00193     
00194     lane0State = REVERSING;
00195     lane0FeedEdge = 0;
00196     lane0LastEdge = LN0_FEEDGATE;
00197     
00198     // go backwards till we smack the stop
00199     feedMotor0.Reverse();
00200 }
00201 
00202 
00203 /*****************************************************************************
00204 **** LANE 1 FEED CONTROL
00205 *****************************************************************************/
00206 
00207 void setLane1Picking()
00208 {
00209     lane1State = PICKING;
00210     
00211     //todo: PwmOut
00212     SOLENOID_1 = 1;
00213 }
00214 
00215 
00216 void setLane1Feeding()
00217 {
00218     lane1State = FEEDING;
00219     lane1FeedEdge = 0;
00220     lane1LastEdge = LN0_FEEDGATE;
00221     
00222     // solenoid release
00223     SOLENOID_1 = 0;
00224     
00225     lane1Feed = 1;
00226     // motor on
00227     feedMotor1.Forward();
00228 }
00229 
00230 void setLane1Reversing()
00231 {
00232     feedMotor1.Brake(); // yes..  this is only going to brake for a few dozen uS...
00233     
00234     lane1Feed = 0;
00235     
00236     lane1State = REVERSING;
00237     lane1FeedEdge = 0;
00238     lane1LastEdge = LN0_FEEDGATE;
00239     
00240     // go backwards till we smack the stop
00241     feedMotor1.Reverse();
00242 }
00243 
00244 void coastLane0()
00245 {
00246     if (lane0State == IDLE)
00247     {
00248         feedMotor0.Coast();
00249     }
00250 }
00251 
00252 void coastLane1()
00253 {
00254     if (lane1State == IDLE)
00255     {
00256         feedMotor1.Coast();
00257     }
00258 }
00259 
00260 void stopLane0Reverse()
00261 {
00262     feedMotor0.Brake();
00263     
00264     lane0Feedout.attach(&coastLane0, 0.010);
00265 }
00266 
00267 void stopLane1Reverse()
00268 {
00269     feedMotor1.Brake();
00270     
00271     lane1Feedout.attach(&coastLane1, 0.010);
00272 }
00273 
00274 /*****************************************************************************
00275 **** MAIN
00276 *****************************************************************************/
00277 
00278 
00279 int main() 
00280 {
00281     // solenoids off
00282     SOLENOID_0 = 0;
00283     SOLENOID_1 = 0;
00284     
00285     // motors high impedance
00286     feedMotor0.Coast();
00287     feedMotor1.Coast();
00288     tapeMotor0.Coast();
00289     tapeMotor1.Coast();
00290     
00291     LED_0 = 1;
00292     LED_1 = 1;
00293         
00294     // give me time to get serial port connected
00295     wait(2);
00296     
00297     LED_0 = 0;
00298     LED_1 = 0;
00299     
00300     // clear terminal screen - ignore compiler warning
00301     pc.printf("\x1Bc\x1B[2J");
00302     
00303     pc.printf("\r\nFeeder POST...\r\n\r\n");
00304     
00305     UID feederID = IAP_ReadUID();
00306     pc.printf("Feeder UID: {%#10x-%#10x-%#10x-%#10x }\r\n\r\n", feederID.word0, feederID.word1, feederID.word2, feederID.word3);
00307     
00308     pc.printf("Gate 0 (L0 Feed): %d\r\n", LN0_FEEDGATE.read());
00309     pc.printf("Gate 1 (L1 Feed): %d\r\n", LN1_FEEDGATE.read());
00310     pc.printf("Gate 2 (L0 Tape): %d\r\n", LN0_TAPEGATE.read());
00311     pc.printf("Gate 3 (L1 Tape): %d\r\n", LN1_TAPEGATE.read());
00312     pc.printf("\r\n---\r\n");
00313     
00314     pc.printf("Green Button 0 (L0): %d\r\n", BUTTON_GREEN_0.read());
00315     pc.printf("Green Button 1 (L1): %d\r\n", BUTTON_GREEN_1.read());
00316     pc.printf("Yellow Button 0 (L0): %d\r\n", BUTTON_YELLOW_0.read());
00317     pc.printf("Yellow Button 1 (L1): %d\r\n", BUTTON_YELLOW_1.read());
00318     pc.printf("\r\n---\r\n");
00319     
00320     pc.printf("Lane 0 Control: %d\r\n", LANEIN_0.read());
00321     pc.printf("Lane 1 Control: %d\r\n", LANEIN_0.read());
00322     pc.printf("\r\n---\r\n");
00323     pc.printf("END\r\n\r\n");
00324     
00325     // make sure motors are at their end stops
00326     feedMotor0.Reverse();
00327     wait_ms(50);
00328     feedMotor0.Coast();
00329     
00330     // one after another, stall current is ~750mA per motor
00331     feedMotor1.Reverse();
00332     wait_ms(50);
00333     feedMotor1.Coast();
00334     
00335     // cover tape pickup interrupts
00336     /* Moved to main loop
00337     LN0_TAPEGATE.rise(&L0_Tape_int);
00338     LN0_TAPEGATE.fall(&L0_Tape_int);
00339     LN1_TAPEGATE.rise(&L1_Tape_int);
00340     LN1_TAPEGATE.fall(&L1_Tape_int);
00341     */
00342     
00343     // button/gate state temp
00344     int b0 = 0;
00345     int b1 = 0;
00346     uint8_t c = 0;
00347     
00348     // timer for button debouncing. oldass hardware buttons are bouncy!
00349     Timer button0Time;
00350     Timer button1Time;
00351     float t0 = 0;
00352     float t1 = 0;
00353     
00354     while(true)
00355     {
00356         /**********************************************************************
00357         ** PC USB COMMS - 2.01uS
00358         **********************************************************************/
00359         // no pc comms checking when feeding, no need
00360         if ((lane0State == IDLE || lane0State == PICKING) && (lane1State == IDLE || lane1State == PICKING))
00361         { 
00362             while (pc.readable())
00363             {
00364                 c = pc.getc();
00365                 pc.putc(c);
00366             
00367                 switch (pcCommand)
00368                 {
00369                     case READYFORCOMMAND:
00370                         if (c == 'F')
00371                         { 
00372                             pcCommand = FEED;
00373                             pc.printf("\r\nFeed: ");
00374                         }
00375                         else if (c == 'P')
00376                         {
00377                             pcCommand = PICK;
00378                             pc.printf("\r\nPick: ");
00379                         }
00380                         else if (c == 'E')
00381                         {
00382                             pcCommand = CLOSE;
00383                             pc.printf("\r\nClose: ");
00384                         }
00385                         else if (c == 'C')
00386                         {
00387                             pcCommand = CONFIG;
00388                             pc.printf("\r\nConfig: ");
00389                         }
00390                         break;
00391                     case FEED:
00392                         if (c == '0')
00393                         {
00394                             pc.printf("\rLane 0 Feeding\r\n");
00395                             setLane0Feeding();
00396                         }
00397                         else if (c == '1')
00398                         {
00399                             pc.printf("\rLane 1 Feeding\r\n");
00400                             setLane1Feeding();
00401                         }
00402                         
00403                         pcCommand = READYFORCOMMAND;
00404                         break;
00405                     case PICK:
00406                         if (c == '0')
00407                         {
00408                             pc.printf("\rLane 0 Picking\r\n");
00409                             
00410                             setLane0Picking();
00411                         }
00412                         else if (c == '1')
00413                         {
00414                             pc.printf("\rLane 1 Picking\r\n");
00415                             
00416                             setLane1Picking();
00417                         }
00418                             
00419                         pcCommand = READYFORCOMMAND;
00420                         break;
00421                     case CLOSE:
00422                         if (c == '0')
00423                         {
00424                             pc.printf("\rLane 0 Closing\r\n");
00425                             
00426                             SOLENOID_0 = 0;
00427                             lane0State = IDLE;
00428                         }
00429                         else if (c == '1')
00430                         {
00431                             pc.printf("\rLane 1 Closing\r\n");
00432                             
00433                             SOLENOID_1 = 0;
00434                             lane1State = IDLE;
00435                         }
00436                             
00437                         pcCommand = READYFORCOMMAND;
00438                         break;
00439                     case CONFIG:
00440                         pcCommandPos++;
00441                         if (pcConfig == NONE)
00442                         {
00443                             if (c == 'L')
00444                                 pcConfig = LANE;
00445                             else
00446                                 pcCommand = READYFORCOMMAND;
00447                         }
00448                         else if (pcConfig == LANE && selectedLane == UNSELECTED)
00449                         {
00450                             if (c == '0')
00451                                 selectedLane = ZERO;
00452                             else if (c == '1')
00453                                 selectedLane = ONE;
00454                             else
00455                             {
00456                                 pcCommand = READYFORCOMMAND;
00457                                 pcConfig = NONE;
00458                             }
00459                         }
00460                         else if (pcConfig == LANE)
00461                         {
00462                             if (selectedLane == ZERO)
00463                                 lane0FeedDistance = c == '1' ? 1 : 2;
00464                             else
00465                                 lane1FeedDistance = c == '1' ? 1 : 2;
00466                                 
00467                             selectedLane = UNSELECTED;
00468                             pcCommand = READYFORCOMMAND;
00469                             pcConfig = NONE;
00470                         }
00471                         break;
00472                     default:
00473                         break;
00474                 }
00475             }
00476         }
00477         
00478         /**********************************************************************
00479         ** BUTTON CHECK - 3.65uS
00480         **********************************************************************/
00481         if (lane0State != FEEDING)
00482         {
00483             b0 = BUTTON_GREEN_0; // read states
00484             b1 = BUTTON_GREEN_1;
00485             
00486             // BUTTON 0
00487             if (b0 == button0LastState && b0) // button0 is pressed and was pressed last time too
00488             {   
00489                 if (lane0State == IDLE)
00490                 {
00491                     if (button0Time.read_ms() > 100) // wait for button state to be stable for 100ms
00492                     {
00493                         button0Time.reset();
00494                         button0Time.stop(); // no need to keep counting
00495                         
00496                         pc.printf("Picking 0\r\n");
00497                         setLane0Picking(); // open solenoid while button is down
00498                     }
00499                 }
00500             }
00501             else if (!b0 && b0 != button0LastState) // low transition - button released
00502             {
00503                 button0LastState = b0;
00504                 
00505                 if (lane0State == PICKING) // not just a bounce
00506                 {
00507                     pc.printf("Feeding 0\r\n");
00508                     setLane0Feeding(); // button has been released, feed one component
00509                 }
00510             }
00511             else // high transition
00512             {
00513                 if (b0) // button pressed
00514                 {
00515                     button0Time.reset();
00516                     button0Time.start();
00517                 }
00518             }
00519             
00520             button0LastState = b0;
00521         }
00522         
00523         if (lane1State != FEEDING)
00524         {
00525             // BUTTON 1
00526             if (b1 == button1LastState && b1) // button0 is pressed and was pressed last time too
00527             {
00528                 if (lane1State == IDLE)
00529                 {
00530                     if (button1Time.read_ms() > 100)
00531                     {
00532                         button1Time.reset();
00533                         button1Time.stop();
00534                         
00535                         pc.printf("Picking 1\r\n");
00536                         setLane1Picking();
00537                     }
00538                 }
00539             }
00540             else if (!b1 && b1 != button1LastState) // low transition - button released
00541             {
00542                 button1LastState = b1;
00543                 
00544                 if (lane1State == PICKING)
00545                 {
00546                     pc.printf("Feeding 1\r\n");
00547                     setLane1Feeding();
00548                 }
00549             }
00550             else // high transition
00551             {
00552                 if (b1)
00553                 {
00554                     button1Time.reset();
00555                     button1Time.start();
00556                 }
00557                 button1LastState = b1;
00558             }
00559         }
00560         
00561         /**********************************************************************
00562         ** ENCODER CHECK - 3.38uS
00563         **********************************************************************/
00564         
00565         b0 = LN0_FEEDGATE;
00566         b1 = LN1_FEEDGATE;
00567         
00568         // LANE 0
00569         if (lane0State == FEEDING || lane0State == REVERSING)
00570         {
00571             if (lane0LastEdge != b0)
00572             {
00573                 lane0FeedEdge++;
00574             }
00575         }
00576         lane0LastEdge = b0;
00577         
00578         // off stop, gap, past stop
00579         if (lane0State == FEEDING && 
00580             lane0FeedEdge > (lane0FeedDistance * FEED_FWD_ENC_PULSE))
00581         {
00582             lane0FeedEdge = 0;
00583             feedMotor0.Brake();
00584             pc.printf("R0\r\n");
00585             setLane0Reversing();
00586         }
00587         // gap, return to stop
00588         else if (lane0State == REVERSING && lane0FeedEdge > 1)
00589         {
00590             lane0State = IDLE;
00591             lane0Feedout.attach(&stopLane0Reverse, 0.020);
00592             pc.printf("Idle 0\r\n");
00593         }
00594         
00595         // LANE 1
00596         if (lane1State == FEEDING || lane1State == REVERSING)
00597         {
00598             if (lane1LastEdge != b1)
00599             {
00600                 lane1FeedEdge++;
00601             }
00602         }
00603         lane1LastEdge = b1;
00604         
00605         // off stop, gap, past stop
00606         if (lane1State == FEEDING && 
00607             lane1FeedEdge > (lane1FeedDistance * FEED_FWD_ENC_PULSE))
00608         {
00609             lane1FeedEdge = 0;
00610             feedMotor1.Brake();
00611             pc.printf("R1\r\n");
00612             setLane1Reversing();
00613         }
00614         // gap, return to stop
00615         else if (lane1State == REVERSING && lane1FeedEdge > 1)
00616         {
00617             lane1State = IDLE;
00618             lane1Feedout.attach(&stopLane1Reverse, 0.020);
00619             pc.printf("Idle 1\r\n");
00620         }
00621         LED_0 = 1;
00622         
00623         /**********************************************************************
00624         ** COVER TAPE CHECK - Gate closed: 41.98uS, Gate open 30.48uS
00625         **********************************************************************/
00626         b0 = LN0_TAPEGATE;
00627         b1 = LN1_TAPEGATE;
00628         if (tape0Takeup)
00629             t0 = tape0PullTime.read(); // very slow..
00630         if (tape1Takeup)
00631             t1 = tape0PullTime.read(); // very slow..
00632         
00633         // LANE 0
00634         if (!b0 && !tape0Takeup) // cover tape slacked enough to interrupt gate
00635         {
00636             tape0PullTime.reset();
00637             tape0PullTime.start();
00638             
00639             tape0Takeup = 1;
00640             tapeMotor0.Forward();
00641         }
00642         else if ((b0 && t0 > TAPE_MIN_PULL_TIME) ||
00643                  (!b0 && tape0Takeup && t0 > TAPE_MAX_PULL_TIME))
00644         {
00645             stopTapeL0();
00646         }
00647         
00648         // LANE 1
00649         if (!b1 && !tape1Takeup) // gate opened
00650         {
00651             tape1PullTime.reset();
00652             tape1PullTime.start();
00653             
00654             tape1Takeup = 1;
00655             tapeMotor1.Forward();
00656         }
00657         else if ((b1 && t1 > TAPE_MIN_PULL_TIME) ||
00658                  (!b1 && tape1Takeup && t1 > TAPE_MAX_PULL_TIME))
00659         {
00660             stopTapeL1();
00661         }
00662         
00663         LED_0 = 0; // for timing check
00664     }
00665 }