File content as of revision 2:830d3c808679:

#include "mbed.h"
#include "CMPS_DAGOZ.h"
#include "EncoderDAGOZ.h"
#include "MotorDAGOZ.h"
#include "EncoderMotor.h"
#include "PID.h"
#include "Constanta.h"
// #include <ros.h>
// #include <rosserial_mbed/HardwareCommandMsg.h>
// #include <rosserial_mbed/HardwareStateMsg.h>

/*****************************
        ROS node handle 
 *****************************/
// ros::NodeHandle nh;

/*****************************
        Pin Declaration
 *****************************/
//Encoder pin
EncoderDAGOZ locomotionEncL(TIM1);          //Locomotion Right Encoder
EncoderDAGOZ locomotionEncR(TIM3);          //Locomotion Left Encoder
EncoderDAGOZ locomotionEncB(TIM2);          //Locomotion Back Encoder
EncoderDAGOZ dribblerEncL(TIM4);            //Dribbler Right Encoder
EncoderDAGOZ dribblerEncR(TIM8);            //Dribbler Left Encoder

EncoderMotor intEncR(PG_8, PE_0, 537.6, EncoderMotor::X4_ENCODING);
EncoderMotor intEncB(PD_10, PG_14, 537.6, EncoderMotor::X4_ENCODING);
EncoderMotor intEncL(PF_2, PF_1, 537.6, EncoderMotor::X4_ENCODING);

//Motor pin buat Board Sistem next ver.
MotorDagoz locomotionMotorL(PF_12, PF_8);   //Locomotion Right Motor
MotorDagoz locomotionMotorR(PG_1, PF_9);    //Locomotion Left Motor
MotorDagoz locomotionMotorB(PF_11, PF_7);   //Locomotion Back Motor

MotorDagoz dribblerMotorR(PF_15, PE_6);     //Dribbler Right Motor
MotorDagoz dribblerMotorL(PE_15, PE_5);     //Dribbler Left Motor


//Serial pin
Serial pc(USBTX, USBRX, 115200);          //Serial debug 
//Compass CMPS12 pin
CMPS_DAGOZ compass(PB_9, PB_8, 0xC0);       //Compass I2C Communication SDA SCL
DigitalOut compassLed(PC_5);                //CMPS error indicator

//Potensio Pin
AnalogIn dribblerPotL(PF_6);                //Potensio for Left Dribbler, di board saat ini masih PC_2
AnalogIn dribblerPotR(PC_3);                //Potensio for Right Dribbler
AnalogIn infraRed(PF_3);                    //Potensio for Kicker, di board saat ini masih PD_4

//LED Pin 
DigitalOut ledRed(PG_4);                    //Red LED
DigitalOut ledGreen(PG_5);                  //Green LED
DigitalOut ledBlue(PG_6);                   //Blue LED

//Kicker Pin            
PwmOut kicker(PB_15);                       //Kicker pwm effort, di board saat ini masih PC_8
//Stepper Pin
DigitalOut stepperDir(PA_10);               //Direction pin for stepper
DigitalOut stepperStep(PF_10);              //Step pin for stepper, di board saat ini masih PC_4

/******************************
  Publisher-Subscriber
 ******************************/
// dagozilla_msgs::HardwareStateMsg stateMsg;
// ros::Publisher statePub("/hardware/state", &stateMsg);

// void commandCallback(const dagozilla_msgs::HardwareCommandMsg& commandMsg);
// ros::Subscriber<dagozilla_msgs::HardwareCommandMsg> commandSub("/hardware/command", &commandCallback);

//PID global object PID, P I D N TS FF MODE
PID locomotionPidR(0.142174, 4.898472, 0, 100, 0.02, 0, PID::PID_MODE);  //Right locomotion PID
PID locomotionPidL(0.110189, 4.861782, 0, 100, 0.02, 0, PID::PID_MODE);  //Left locomotion PID
PID locomotionPidB(0.142982, 4.840596, 0, 100, 0.02, 0, PID::PID_MODE);  //Back locomotion 

//thread for RTOS
Thread thread1;
Thread thread2;

//primitive function
void mainProcess();
void getCompass();
void moveLocomotion();
void moveDribbler();
void publishMessage();

bool dribbler_state;

/*****************************
 Main Function
 *****************************/

int main()
{
//init ros advertise and subscribe
    // nh.initNode();
    // nh.advertise(statePub);
    // nh.subscribe(commandSub);

    kicker.period(0.01f);
    kicker = 1;   //deactivate kicker, active LOW

    thread1.start(mainProcess);
    thread2.start(getCompass);

    while (true) {
        //do nothing
    }
}

void mainProcess(){
    float cur_pot_L0 = ((float)dribblerPotL.read()*100.0f);
    float cur_pot_R0 = ((float)dribblerPotR.read()*100.0f);
    Thread::wait(1000);
    
    float rotInR, rotInL, rotInB;
    
    while(1){
        // nh.spinOnce();
        cur_pot_L = ((float)dribblerPotL.read()*100.0f-cur_pot_L0);
        cur_pot_R = -((float)dribblerPotR.read()*100.0f-cur_pot_R0);
        //ball distance from IR
        distance = infraRed.read();
        //read encoder
        cur_locomotion_R = -locomotionEncR.GetCounter(1)/4000.0;
        cur_locomotion_L = -locomotionEncL.GetCounter(1)/4000.0;
        cur_locomotion_B = -locomotionEncB.GetCounter(1)/4000.0;
        
        rotInB = intEncR.getRevolutions();
        rotInR = intEncL.getRevolutions();
        rotInL = intEncB.getRevolutions();
        
        cur_dribbler_R = dribblerEncR.GetCounter(1)/537.6;
        cur_dribbler_L = -dribblerEncL.GetCounter(1)/537.6;
        // Correct encoder unit
        locomotion_R_rot = cur_locomotion_R * LOCOMOTIONWHEEL * 2 * PI; // in m
        locomotion_L_rot = cur_locomotion_L * LOCOMOTIONWHEEL * 2 * PI; // in m
        locomotion_B_rot = cur_locomotion_B * LOCOMOTIONWHEEL * 2 * PI; // in m
        dribbler_R_rot = cur_dribbler_L * DRIBBLERWHEEL * 2 * PI; // in m
        dribbler_L_rot = cur_dribbler_R * DRIBBLERWHEEL * 2 * PI; // in m
        // Calculate acutal velocity
        locomotion_R_vel = (rotInR*2*PI*0.05) / 0.02;
        locomotion_L_vel = (rotInL*2*PI*0.05) / 0.02;
        locomotion_B_vel = (rotInB*2*PI*0.05) / 0.02;
        dribbler_R_vel = dribbler_R_rot / 0.02;
        dribbler_L_vel = dribbler_L_rot / 0.02;
        
//        kicker conditional
//        if(kick_power_target > 0) {
//            kicker.write(1-kick_power_target);    //Active LOW
//            Thread::wait(10);
//            kicker = 1;
//            kick_power_target = 0;
//        } 
        //moveLocomotion();
        moveDribbler();     //active dribbler state
        
        // publishMessage();
        // nh.spinOnce();   
        Thread::wait(20);
    }
}

void getCompass(){
    float theta0 = compass.readBearing()/10.0;
    Thread::wait(1000);
    theta0 = compass.readBearing()/10.0;
    compassLed = 0;
    while(1){
        float theta_temp = (compass.readBearing()/10.0) - theta0;
        if(theta_temp > 180.0 && theta_temp <= 360.0)
            theta_com = ((compass.readBearing()/10.0) - 360.0 - theta0)/-RADTODEG;    
        else if(theta_temp < -180.0 && theta_temp >= -360.0)
            theta_com = ((compass.readBearing()/10.0) + 360.0 - theta0)/-RADTODEG;
        else
            theta_com = ((compass.readBearing()/10.0) - theta0)/-RADTODEG;
        
        if(theta != 0) compassLed = 1;
        else compassLed = 0;
        
        theta = theta_com;

        if(theta != 0) compassLed = 1;
        else compassLed = 0;

        Thread::wait(60);
    }
}

void moveLocomotion(){
    // Calculate motor pwm
    locomotion_R_target_rate = locomotionPidR.createpwm(locomotion_R_vtarget, locomotion_R_vel);
    locomotion_L_target_rate = locomotionPidL.createpwm(locomotion_L_vtarget, locomotion_L_vel);
    locomotion_B_target_rate = locomotionPidB.createpwm(locomotion_B_vtarget, locomotion_B_vel);
    
    //Motor pwm    
    locomotionMotorR.setpwm(locomotion_R_target_rate);
    locomotionMotorL.setpwm(locomotion_L_target_rate);
    locomotionMotorB.setpwm(locomotion_B_target_rate);
}

void moveDribbler(){
    float RKXFF, LKXFF;                 //Dribbler x velocity feedforward
    float RKYFF, LKYFF;                 //Dribbler y velocity feedforward
    
    if (vx <= 0){
        RKXFF = 0.9;    LKXFF = 0.3;
    } else{
        RKXFF = 0.3;    LKXFF = 0.9;
    }

    if (vy > 0){
        RKYFF = 1;    LKYFF = 1;
    } else{
        RKYFF = 1.3;    LKYFF = 1.3;
    }

    //Compute value//
    vx = (locomotion_B_vel*2/3) + (-locomotion_R_vel/3) + (-locomotion_L_vel/3);
    vy = (locomotion_R_vel*2/3) + (-locomotion_L_vel*2/3);
    w = (locomotion_B_vel+locomotion_L_vel+locomotion_R_vel)/(3*0.21);
    //Calculate Feedback and FeedForward 
    //need correction for +- in each robot velocity
    float dribbler_R_vel_target =  (LTARGET - cur_pot_L)*LK1FB+(fabs(vx*LKXFF) - (vy*LKYFF) + fabs(w*LKTFF));
    float dribbler_L_vel_target =  (RTARGET - cur_pot_R)*RK1FB+(fabs(vx*RKXFF) - (vy*RKYFF) + fabs(w*RKTFF));
    dribbler_L_target_rate = ((dribbler_L_vel_target - dribbler_L_vel)*LK2FB)+(LKVFF*(dribbler_L_vel_target));
    dribbler_R_target_rate = ((dribbler_R_vel_target - dribbler_R_vel)*RK2FB)+(RKVFF*(dribbler_R_vel_target));

    if(dribbler_R_target_rate>1.0) dribbler_R_target_rate = 1.0;
    if(dribbler_L_target_rate>1.0) dribbler_L_target_rate = 1.0;

    if(abs(LTARGET - cur_pot_L)<=5.0) dribbler_R_target_rate = 0.0;
    if(abs(RTARGET - cur_pot_R)<=5.0) dribbler_L_target_rate = 0.0;
    /*pc.printf("targetka=%.2f targetki=%.2f potka=%.2f potki=%.2f xka=%.2f xki=%.2f yka=%.2f yki=%.2f wka=%.2f wki=%.2f\n",
    dribbler_R_vel_target, dribbler_L_vel_target,
    (RTARGET - cur_pot_R)*RK1FB, (LTARGET - cur_pot_L)*LK1FB, 
    fabs(vx*RKXFF), fabs(vx*LKXFF), 
    (-vy*RKYFF), (-vy*LKYFF), 
    fabs(w*RKTFF), fabs(w*LKTFF));*/
    
    if(dribbler_state){
        dribblerMotorL.setpwm(dribbler_L_target_rate);
        dribblerMotorR.setpwm(dribbler_R_target_rate);     
    } else {
        dribblerMotorL.setpwm(0);
        dribblerMotorR.setpwm(0); 
    }
    dribblerMotorL.setpwm(dribbler_L_target_rate);
    dribblerMotorR.setpwm(dribbler_R_target_rate); 
    
}

// void publishMessage(){
//     // Publish position data
//     stateMsg.base_wheel_right_position = locomotion_R_rot;    // in m
//     stateMsg.base_wheel_back_position = locomotion_B_rot;     // in m
//     stateMsg.base_wheel_left_position = locomotion_L_rot;     // in m
//     // Publish velocity data
//     stateMsg.base_wheel_right_velocity = locomotion_R_vel;    // in m/s
//     stateMsg.base_wheel_back_velocity = locomotion_B_vel;     // in m/s
//     stateMsg.base_wheel_left_velocity = locomotion_L_vel;     // in m/s
//     // Publish distance data
//     stateMsg.dribbler_ir_distance = distance;               // in analog value
//     stateMsg.compass_angle = theta;
//     stateMsg.dribbler_active = dribbler_state;
//     statePub.publish(&stateMsg);
// }

// void commandCallback(const dagozilla_msgs::HardwareCommandMsg& commandMsg) {
//       locomotion_R_vtarget = commandMsg.base_wheel_right_velocity;
//       locomotion_B_vtarget = commandMsg.base_wheel_back_velocity;
//       locomotion_L_vtarget = commandMsg.base_wheel_left_velocity;
//       kick_power_target = commandMsg.kicker_effort;
//       dribbler_state = commandMsg.dribbler_active;
// }