File content as of revision 7:b878dce06561:

#include "mbed.h"
#include "rtos/rtos.h"
#include "Teseo-LIV3F.h"
#include <cstdlib>
#include <vector>
#include "XNucleoIKS01A2.h"

#include "Servo.h"

#include "LSCServo.h"
#include "DC_Motor_Controller.h"

#include <ros.h>
#include "std_msgs/Bool.h"
#include "geometry_msgs/Twist.h"

#define BAUD_RATE 115200

using namespace std;
using namespace std_msgs;
using namespace ros;
using namespace geometry_msgs;

DC_Motor_PID motor(D6,D3,D4,D5,792); //out1 out2 in1 in2 ppr

DigitalOut myled = LED1;
DigitalOut Refer_Volt_3_3 = PB_1;//the left pin of D4
DigitalOut pusher2 = D13;
DigitalOut pusher1 = D12;         //not in use
DigitalIn but(D7);              // firing pin
DigitalOut up = D11; //pneumatic part
DigitalOut down = D10; //pneumatic part

LSCServo robot_arm(D1, D0); //D1 = Tx, D0 = Rx

NodeHandle nh;      //ROS node

static Thread t1, t2, t3, t4, t5, t6, t7;
Semaphore s1(0), s2(0), s3(0), s4(0), s5(0), s6(0), s7(0);


//thread function
void movingArmLeft()
{
    while(1){
        s1.wait();
            robot_arm.moveServos(3,1000,2,185,3,540,4,1000); // rotate down, tuned
            robot_arm.moveServos(3,1500,2,650,3,180,4,1000); // rotate left, tuned
            Thread::wait(1000);
            robot_arm.moveServos(3,1000,2,650,3,180,4,500); // open the crawler
    }
}
void movingArmRight()
{
    while(1){
        s2.wait();
            robot_arm.moveServos(3,1000,2,185,3,540,4,1000);
            robot_arm.moveServos(3,1500,2,650,3,920,4,1000); // rotate left, tuned
            Thread::wait(1000);
            robot_arm.moveServos(3,1500,2,700,3,920,4,500);
    }
}
void movingArmOpen()
{
    while(1){
        s3.wait();
            robot_arm.moveServos(3,1000,2,185,3,540,4,500);      // initial pos to put an arrow
    }
}
void movingArmClose()
{
    while(1){
        s4.wait();
            robot_arm.moveServos(3,1000,2,185,3,540,4,1000); // catch/crawl the arrow
    }
}
void moving20cm()
{
    while(1){
        s5.wait();
        motor.set_out(0,0.4);
        while(1)
        {
           if(but){
                motor.set_out(0,0);
                break;
           }
        }
        motor.reset();
        motor.move_angle(-250);
    }
}
void moving10cm(){
    s6.wait();
    motor.move_angle(-100);
}
void movingOrigin(){
    while(1){
        s7.wait();
        motor.set_out(0,1);
        while(1)
        {
            if(but) {
                motor.set_out(0,0);
                break;
            }
        }
        motor.reset();
        motor.move_angle(-250);
    }   
}
//ros function
void messagesqu(const Bool& _msg){  //callback of square, open claw 
    bool check = _msg.data;
    if(check)
        s3.release();
}

void messagecro(const Bool& _msg){  //callback of cross, arrow point to back
    bool check = _msg.data;
    if(check)
        s2.release();
}

void messagecir(const Bool& _msg){  //callback of circle, arrow point to front
    bool check = _msg.data;
    if(check)
        s1.release();
}

void messagetri(const Bool& _msg){  //callback of triangle, close claw
    bool check = _msg.data;
    if(check)
        s4.release();
}

void messageL1(const Bool& _msg){   //callback of L1  up pusher
    bool check = _msg.data;
    if(check){
        pusher1 = 1;
        }
    else
        pusher1 = 0;
}

void messageR1(const Bool& _msg){   //callback of R1, up and turn
    bool check = _msg.data;
    if(check){
        pusher2 = 1;
        }
    else
        pusher2 = 0;
}

void messageL3(const Bool& _msg){   //callback of R3,turn 20 cm
    bool check = _msg.data;
    if(check){
        s5.release();
        }
}

void messageR3(const Bool& _msg){   //callback of R3,turn 10 cm
    bool check = _msg.data;
    if(check){
        s6.release();
        }
}

void messageps(const Bool& _msg){   //callback of ps,turn back to 0 pulse
    bool check = _msg.data;
    if(check)
        s7.release();
}

void messagepad(const Bool& _msg){   //callback of ps, not in use for now   arm go down and release
    bool check = _msg.data;
    if(check){
        robot_arm.moveServos(3,1500,2,500,3,540,4,1000);
        }
}

void messagekeypad (const Twist& _msg){     //callback of keypad
    float x = _msg.linear.x;
    float y = _msg.linear.y;
    if (x==-1){                                 //  Rotate motor clockwise
        motor.set_out(1, 0);
    }
    else if(x==1) {                          //  Rotate motor anti-clockwrise 
        motor.set_out(0, 1);
    }
    else{
        motor.set_out(0, 0);
    }
    if (y==1){                                  //  pump air of pnumetic part
        up = 1;
    }
    else if(y==-1) {                          //  release air of pnumetic part
        down = 1;
    }
    else{
        up = 0;
        down = 0;
    }
}

Subscriber<Bool> subsqu("button_square", &messagesqu);
Subscriber<Bool> subcro("button_cross", &messagecro);
Subscriber<Bool> subcir("button_circle", &messagecir);
Subscriber<Bool> subtri("button_triangle", &messagetri);
Subscriber<Bool> subL1("button_L1", &messageL1);
Subscriber<Bool> subR1("button_R1", &messageR1);
Subscriber<Bool> subL3("button_L3", &messageL3);
Subscriber<Bool> subR3("button_R3", &messageR3);
Subscriber<Bool> subps("button_ps", &messageps);
Subscriber<Bool> subpad("button_pad", &messagepad);
Subscriber<Twist> subkeypad("button_keypad", &messagekeypad);

int main() {
    myled = 0;
    but.mode(PullUp);
    
    motor.set_pid(0.008, 0, 0, 0.10);
    
    nh.getHardware()->setBaud(BAUD_RATE);   //set baud rate
    nh.initNode();
    
    nh.subscribe(subsqu);
    nh.subscribe(subcro);
    nh.subscribe(subcir);
    nh.subscribe(subtri);
    nh.subscribe(subL1);
    nh.subscribe(subR1);
    nh.subscribe(subL3);
    nh.subscribe(subR3);
    nh.subscribe(subkeypad);
    nh.subscribe(subps);
    nh.subscribe(subpad);
    t1.start(&movingArmLeft);
    t2.start(&movingArmRight);
    t3.start(&movingArmOpen);
    t4.start(&movingArmClose);
    t5.start(&moving20cm);
    t6.start(&moving10cm);
    t7.start(&movingOrigin);
    while(true) {
        nh.spinOnce();
        if(nh.connected())
            myled = 1;
        else
            myled = 0;
    }
}