File content as of revision 6:23b1ed826b59:

#include "mbed.h"
#include "MODSERIAL.h"
#define SERIAL_BAUD 115200
#include "motordriver.h"
#include "QEI.h"
#include "Servo.h"

//======== Serial Communication ================================================
MODSERIAL pc(USBTX,USBRX);

//======== Motor and QEI =======================================================
int Brakeable;
int sign;

// motor
Motor Cart(D5, D4, D4, Brakeable);      // right motor
Motor Arm(D6,D7, D7, Brakeable);        // left motor

// qei
QEI Encoder_Cart(D10, D11, NC, 6400);
QEI Encoder_Arm(D12, D13, NC, 6400);

// servo
Servo servo(D9);

//======== Miscellaneous =======================================================
// button
InterruptIn btn(SW2);
InterruptIn btn2(SW3);

InterruptIn btn_cart(D1);
InterruptIn btn_arm(D2);
InterruptIn btn_claw(D3);

// led
DigitalOut led_r(LED_RED);
DigitalOut led_g(LED_GREEN);
DigitalOut led_b(LED_BLUE);

// potmeter
AnalogIn pot_cart(A2);                  
AnalogIn pot_arm(A3);  

// ticker
Ticker tick_part;                       // ticker to switch parts        

//======== Variables ===========================================================
// counters
int num_turned_on_0 = 0;                // count number of times red LED turned on
int num_turned_on_1 = 0;                // count number of times green LED turned on
int num_turned_on_2 = 0;                // count number of times blue LED turned on

int num_claw_turned_on_0 = 0;           // count number of times red LED turned on
int num_claw_turned_on_1 = 0;           // count number of times green LED turned on
int num_claw_turned_on_2 = 0;           // count number of times blue LED turned on

// speed
double cart_speed = 0.3;
double arm_speed = 0.1;

// position
float factor_cart = 0.06559;
float factor_arm = 0.1539;
int position_cart;
int position_arm;
float ain_cart;     //Variable to store the analog input of the cart
float ain_arm;      //Variable to store the analog input of the arm

int position_claw;


// miscellaneous
const float kTimeToggle = 0.25f;        // period with which to toggle the parts
const int LedOn = 0;                    // LED on if 0
volatile int part_id = 2;               // ID of what part should move, begins with the cart
volatile int servo_id = 1;              // ID to the side the servo should move, begins in center position


//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//======== Functions and main ==============================================================

// Switch between Cart, Arm and Claw
void SwitchPart()
{
    switch (part_id) {
        //Cart
        case 2: {
            led_r = LedOn;          
            if(led_r == LedOn){
                num_turned_on_0++;
            
                if(btn && btn2) {                  
                    Arm.speed(0) == 0;
                    Cart.speed(0) == 0;                           
                
                }else if (btn && !btn2) {
                    if(position_cart <= -105){                                  //If the cart is at the right side, it stops
                    Cart.stop(1)==1;
                    
                    }else if(position_cart <= -90){                             //If the cart is at 90 mm at the right, it slows down
                    Cart.speed(0.2)==0.2;
                    
                    }else if(position_cart >= 105 && position_arm <=-45){       //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
                    Cart.stop(1) == 1;
                    
                    }else if(position_cart >= 105 && position_arm <=-35 && position_claw == -18){
                    Cart.stop(1) == 1;    
    
                    }else{
                    Cart.speed(cart_speed)==cart_speed;
                    }
         
                }else if (!btn && btn2) {
                    if(position_cart >= 105){                                   //If the cart is at the left side, it stops
                    Cart.stop(1)==1;
                    
                    }else if(position_cart >= 90){                             //If the cart is at the right side, it stops
                    Cart.speed(-0.2)==-0.2;
                    
                    }else if(position_cart <= -105 && position_arm >=45){       //If the cart is at the right side and the arm is rotated 60 degrees to the right, the cart can't move to the left.
                    Cart.stop(1)==1;
                      
                    }else if(position_cart <= -105 && position_arm >=35 && position_claw == 27){
                    Cart.stop(1)==1;
                    
                    }else{
                    Cart.speed(-cart_speed)==-cart_speed;
                    }
                                            
                }else {
                    Arm.speed(0) == 0;
                    if(position_cart<0){
                    Cart.speed(-0.1)== -0.1;
                    }else if(position_cart>0){
                    Cart.speed(0.1)==0.1;
                    }else{
                    Cart.stop(0)==0;
                    }
                     
                }
            }
            // controle LED    
            led_g = not LedOn;
            led_b = not LedOn;     
            
            // encoder
            position_cart = (Encoder_Cart.getPulses()*factor_cart) ;    
            ain_cart = pot_cart.read();
   
                if (ain_cart == 0){
                Encoder_Cart.reset();   
                }else {}
        
            wait(0.1);
            pc.baud(115200);
            pc.printf("Distance in mm: %i\n", position_cart);
            
            break;
        }
        
        //Arm
        case 3: {
            led_g = LedOn;          
            if(led_g == LedOn){
                num_turned_on_1++;
            
                if(btn && btn2) {            
                    Arm.speed(0) == 0; 
                    Cart.speed(0) == 0;                           
                }
            
                else if (btn && !btn2) {
                    if(position_cart > -105 && position_arm >= 45){             //If the cart is not at the end, the arm can't move any further than 45 degrees
                    Arm.stop(1)==1;
                    
                    }else if(position_cart > -105 && position_arm >= 25 && position_claw == 27){
                    Arm.stop(1)==1;
                        
                    }else if(position_cart<= -105 && position_arm>=80){         //If the cart is at the right end, the arm can't move any further than 70 degrees
                    Arm.stop(1)==1;
    
                    }else{
                    Arm.speed(arm_speed)==arm_speed;
                    } 
         
                }else if (!btn && btn2) {
                    if(position_cart < 105 && position_arm <= -45){             //If the cart is not at the end, the arm can't move any further than 45 degrees 
                    Arm.stop(1)==1;
                    
                    }else if(position_cart < 105 && position_arm <= -25 && position_claw == -18){
                    Arm.stop(1)==1;
                    
                    }else if(position_cart>=105 && position_arm<=-80){          //If the cart is at the left end, the arm can't move any further than 70 degrees
                    Arm.stop(1)==1;
    
                    }else{
                    Arm.speed(-arm_speed)==-arm_speed;
                    } 
                        
                }else {
                Cart.speed(0) == 0;
                if(position_arm>0){
                Arm.speed(-0.1)== -0.1;
                }else if(position_arm<0){
                Arm.speed(0.1)==0.1;
                }else{
                Arm.stop(0)==0;
                    }    
                }
            } 
            // controle LED   
            led_r = not LedOn;
            led_b = not LedOn;           
            
            // encoder
            position_arm = (Encoder_Arm.getPulses()*factor_arm) ;    
            ain_arm = pot_arm.read();
   
                if (ain_arm == 0){
                Encoder_Arm.reset();   
                }else {}
        
            wait(0.1);
            pc.baud(115200);
            pc.printf("Degrees: %i\n", position_arm);
            
            break;
        }
        
        //Claw
        case 4: {
            led_b = LedOn;
            if(led_b == LedOn){
                num_turned_on_2++;
                
                if(btn && btn2){
                       
                }else if(btn && !btn2){
                 servo_id ++;
                 
                    switch (servo_id) {
                        case 0: {
                            led_r = LedOn;
                            if (led_r == LedOn) {
                                num_claw_turned_on_0++;
                            }
                            
                            led_b = not LedOn;
                            led_g = not LedOn;
                            
                            servo.position(-18);
                            pc.printf("Servo position is: left \r\n");
                            break;
                        }
                        case 1: {
                            led_b = LedOn;
                            if (led_b == LedOn) {
                                num_claw_turned_on_1++;
                            }
                            
                            led_r = not LedOn;
                            led_g = not LedOn;
                            
                            servo.position(3);
                            pc.printf("Servo position is: center \r\n");
                            break;
                        }
                        case 2: {
                            led_g = LedOn;
                            if (led_g == LedOn) {
                                num_claw_turned_on_2++;
                            }
                            
                            led_r = not LedOn;
                            led_b = not LedOn;
                            
                            servo.position(27);
                            pc.printf("Servo position is: right \r\n");
                            break;
                        }
                    }
                 
                 
                }else if(!btn && btn2){
                servo_id --;
                
                switch (servo_id) {
                    case 0: {
                        led_r = LedOn;
                        if (led_r == LedOn) {
                            num_claw_turned_on_0++;
                        }
                        
                        led_b = not LedOn;
                        led_g = not LedOn;
                        
                        servo.position(-18);
                        pc.printf("Servo position is: left \r\n");
                        break;
                    }
                    case 1: {
                        led_b = LedOn;
                        if (led_b == LedOn) {
                            num_claw_turned_on_1++;
                        }
                        
                        led_r = not LedOn;
                        led_g = not LedOn;
                        
                        servo.position(3);
                        pc.printf("Servo position is: center \r\n");
                        break;
                    }
                    case 2: {
                        led_g = LedOn;
                        if (led_g == LedOn) {
                            num_claw_turned_on_2++;
                        }
                        
                        led_r = not LedOn;
                        led_b = not LedOn;
                        
                        servo.position(27);
                        pc.printf("Servo position is: right \r\n");
                        break;
                    }
                }
                
                }else{}       
            }
            led_r = not LedOn;
            led_g = not LedOn;                
            
            position_claw = servo.read();
            
            break;
        }
    }
}


// Switch the part
void SetValue2() {
    part_id = 2;
}

void SetValue3() {
    part_id = 3;
}


void SetValue4() {
    part_id = 4;
}


// Main
int main()
{
    led_r = not LedOn;
    led_g = not LedOn;
    led_b = not LedOn;
    
    tick_part.attach(&SwitchPart,kTimeToggle); 
   
    btn_cart.fall(&SetValue2);
    btn_arm.fall(&SetValue3);
    btn_claw.fall(&SetValue4);

    while (true);
}