File content as of revision 0:5e54c5a00e68:

// Demonstrates complete use of the WDDDS systems  and functionalities:
// Cup dispenser, dispensing drink 1, dispensing drink 2, and conveyor belt.
// This code:
// - dispenses a cup, moves the cup under the drink dispenser, dispenses drink 1
//      then moves cup further along treads when button 1 is pressed
// - dispenses a cup, moves the cup under the drink dispenser, dispenses drink 2
//      then moves cup further along treads when button 2 is pressed
// - 4th button is used for unjamming the cup dispenser, as it will extend the
//      linear actuator further to push cups out.

#include "mbed.h"
#include "Motor.h"
#include "Servo.h"

Servo poker(p22);
DigitalIn stby(p25);
Motor tread(p24, p12, p11);         // pwm, fwd, rev (H-bridge)
Motor pump(p26, p13, p14);          // pwm, fwd, rev (H-bridge)
DigitalOut tread2(p21);             // MOSFET
BusOut myled(LED1,LED2,LED3,LED4);  // check for which button is pressed
Serial blue(p28,p27);               // Bluetooth
float cupos = 850;
void cupdispense();
void drink1();
void drink2();
void treads(float time); 

int main()
{
    poker.Enable(1500,20000);
    poker.SetPosition(cupos);
    wait(.2);
     poker.SetPosition(cupos+10);
    stby.mode(PullUp);
    char bnum=0;
    char bhit=0;
    while(1) {
        if (blue.getc()=='!') {
            if (blue.getc()=='B') { //button data packet
                bnum = blue.getc(); //button number
                bhit = blue.getc(); //1=hit, 0=release
                if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                    myled = bnum - '0'; //current button number will appear on LEDs
                    switch (bnum) {
                        case '1': //number button 1
                            if (bhit=='1') {
                                //add hit code here
                                drink1();
                                
                            } else {
                                //add release code here
                            }
                            break;
                        case '2': //number button 2
                            if (bhit=='1') {
                                //add hit code here
                                 drink2();
                            } else {
                                //add release code here
                            }
                            break;
                        case '3': //number button 3
                            if (bhit=='1') {
                                //add hit code here
                            } else {
                                //add release code here
                            }
                            break;
                        case '4': //number button 4
                            if (bhit=='1') {
                                //add hit code here
                                cupdispense();
                            } else {
                                //add release code here
                            }
                            break;
                        default:
                            break;
                    }
                }
            }
        }
    }
}

void cupdispense(){
   if( cupos <2500) {
       cupos += 52;
       wait_ms(400);
       poker.SetPosition(cupos);
       cupos += 53;
       poker.SetPosition(cupos);
       wait_ms(100);
       }
}

void drink1(){
  //  cupdispense();
    //wait(3.5);
    treads(.68);
    wait(.4);
    pump.speed(1);
    wait(2);
    pump.speed(0);
    wait(5);
    treads(1);
}

void drink2(){
  //  cupdispense();
    //wait(3.5);
    treads(.68);
    wait(.4);
    pump.speed(-1);
    wait(2);
    pump.speed(0);
    wait(5);
    treads(1);
}

void treads(float time) {
    tread.speed(.4);
    tread2 = 1;
    wait(time);
    tread.speed(0);
    tread2 = 0;
}