File content as of revision 1:8b6487805a90:

#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 locomotionEncR(TIM1);          //Locomotion Right Encoder
EncoderDAGOZ locomotionEncL(TIM3);          //Locomotion Left Encoder
EncoderDAGOZ locomotionEncB(TIM2);          //Locomotion Back Encoder
EncoderDAGOZ dribblerEncR(TIM4);            //Dribbler Right Encoder
EncoderDAGOZ dribblerEncL(TIM8);            //Dribbler Left Encoder

EncoderMotor intEncL(PE_0, PG_8, 537.6, EncoderMotor::X4_ENCODING);
EncoderMotor intEncR(PG_14, PD_10, 537.6, EncoderMotor::X4_ENCODING);
EncoderMotor intEncB(PF_1, PF_2, 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
DigitalOut compassLed(PC_5);                //CMPS error indicator

//Potensio Pin
AnalogIn dribblerPotR(PF_6);                //Potensio for Left Dribbler, di board saat ini masih PC_2
AnalogIn dribblerPotL(PC_3);                //Potensio for Right Dribbler
AnalogIn kickerPot(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
//Infrared Pin
AnalogIn infraRed(PF_5);                    //Ball distance IR, belum ada di board saat ini
//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 locomotionPidR(0.42210959642179, 0, 0, 100, 0.02);   //Right locomotion PID
PID locomotionPidL(0.408433282254394, 0, 0, 100, 0.02);  //Left locomotion PID
PID locomotionPidB(0.409120668853316, 0, 0, 100, 0.02);  //Back locomotion PID
// PID dribblerPidL(0.279684770785072, 7.8707254128385, 0, 100, 0.02);     //Right dribbler PID
// PID dribblerPidR(0.275600964975253, 7.51195777371376, 0, 100, 0.02);    //Left dribbler PID

//thread for RTOS
Thread thread1;
Thread thread2;

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

/*****************************
 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);
    
    int rotR, rotL, rotB;
    int rotInR, rotInL, rotInB;
    int rotDbR, rotDbL;
    
    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;
    
        rotR = locomotionEncR.GetCounter(0);
        rotL = locomotionEncL.GetCounter(0);
        rotB = locomotionEncB.GetCounter(0);
        rotDbR = dribblerEncR.GetCounter(0);
        rotDbL = dribblerEncL.GetCounter(0);
        
        rotInR += intEncR.getPulses();
        rotInL += intEncL.getPulses();
        rotInB += intEncB.getPulses();
        
//        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 = locomotion_R_rot / 0.002;
        locomotion_L_vel = locomotion_L_rot / 0.002;
        locomotion_B_vel = locomotion_B_rot / 0.002;
        dribbler_R_vel = dribbler_R_rot / 0.002;
        dribbler_L_vel = dribbler_L_rot / 0.002;

        //movement
        //moveLocomotion();
        //moveDribbler();
        
        //kicker conditional
//        if(kick_power_target > 0) {
//            kicker.write(1-kick_power_target);    //Active LOW
//            Thread::wait(10);
//            kicker = 1;
//            kick_power_target = 0;
//        } 

//        publishMessage();
//        nh.spinOnce();
        pc.printf("%d, %d\n", rotDbR, rotDbL);
//        pc.printf("vx:%.2f vy:%.2f w:%.2f dR:%.2f dL:%.2f pR:%.2f pL:%.2f\n", vx, vy, w, dribbler_R_target_rate, dribbler_L_target_rate, cur_pot_R, cur_pot_L);
        
//        locomotionMotorR.setpwm(0.3);
//        locomotionMotorB.setpwm(0.3);
//        locomotionMotorL.setpwm(0.3);

//        dribblerMotorR.setpwm(0.3);
//        dribblerMotorL.setpwm(0.3);        
        Thread::wait(10);
    }
}

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;
        theta_prev = theta_com;
        
        Thread::wait(50);
    }
}

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);
    //Compute value
    x = x_prev + (2*locomotion_B_rot - locomotion_L_rot - locomotion_R_rot)/3*cos(theta_prev) - (-locomotion_L_rot+locomotion_R_rot)*0.5773*sin(theta_prev);
    y = y_prev + (2*locomotion_B_rot - locomotion_L_rot - locomotion_R_rot)/3*sin(theta_prev) + (-locomotion_L_rot+locomotion_R_rot)*0.5773*cos(theta_prev);
    theta = theta_prev + (locomotion_R_rot + locomotion_B_rot + locomotion_L_rot)/(3*0.116);
    //update value
    x_prev = x;         y_prev = y;         theta_prev = theta;
    //Robot Velocity
    vx = (x - x_prev)/0.01;      vy = (y - y_prev)/0.01;      w = (theta - theta_prev)/0.01;
    //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;
    if (vx <= 0){
        RKXFF = 1;
        LKXFF = 0.5;
    } else{
        RKXFF = 0.5;
        LKXFF = 1;
    }
    //Calculate Feedback and FeedForward 
    //need correction for +- in each robot velocity
    dribbler_R_target_rate =  (abs(LTARGET - cur_pot_L)*LK1FB)+(abs(vx*LKXFF) - (vy*LKYFF) + abs(w*LKTFF));
    dribbler_L_target_rate =  (abs(RTARGET - cur_pot_R)*RK1FB)+(abs(vx*RKXFF) - (vy*RKYFF) + abs(w*RKTFF));
    //dribbler_L_target_rate = ((targetVelL - dribblerLVel)*LK2FB)+(LKVFF*(targetVelL-prevTargetVelL));
    //dribbler_R_target_rate = ((targetVelR - dribblerRVel)*RK2FB)+(RKVFF*(targetVelR-prevTargetVelR));
    // prevTargetVelL = targetVelL;
    // prevTargetVelR = targetVelR;
    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)<=2.0) dribbler_R_target_rate = 0.0;
    if(abs(RTARGET - cur_pot_R)<=2.0) dribbler_L_target_rate = 0.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
//    stateMsg.dribbler_wheel_left_position = dribbler_L_rot;   // in m
//    stateMsg.dribbler_wheel_right_position = dribbler_R_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
//    stateMsg.dribbler_wheel_left_velocity = dribbler_L_vel;   // in m/s
//    stateMsg.dribbler_wheel_right_velocity = dribbler_R_vel;  // in m/s
//    // Publish distance data
//    stateMsg.dribbler_ir_distance = distance;               // in analog value
//    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;
      dribbler_L_vtarget = commandMsg.dribbler_wheel_left_velocity;
      dribbler_R_vtarget = commandMsg.dribbler_wheel_left_velocity;
      kick_power_target = commandMsg.kicker_effort;
}