Diff: main.cpp

Revision:

1:8b6487805a90

Parent:

0:1c22457d4aed

Child:

2:830d3c808679

--- a/main.cpp	Thu Jan 17 05:22:45 2019 +0000
+++ b/main.cpp	Thu Feb 28 11:00:40 2019 +0000
@@ -1,46 +1,279 @@
 #include "mbed.h"
-#include "Nucleo_Encoder_16_bits.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
 
-Nucleo_Encoder_16_bits encoder1(TIM1);
-Nucleo_Encoder_16_bits encoder2(TIM2);
-Nucleo_Encoder_16_bits encoder3(TIM3);
-Nucleo_Encoder_16_bits encoder4(TIM4);
-Nucleo_Encoder_16_bits encoder5(TIM8);
-Nucleo_Encoder_16_bits encoder6(TIM9);
-Nucleo_Encoder_16_bits encoder7(TIM12);
+//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 print_char()
+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()
 {
-    int a1 = encoder1.GetCounter(0);
-    int a2 = encoder2.GetCounter(0);
-    int a3 = encoder3.GetCounter(0);
-    int a4 = encoder4.GetCounter(0);
-    int a5 = encoder5.GetCounter(0);
-    int a6 = encoder6.GetCounter(0);
-    int a7 = encoder7.GetCounter(0);
-    printf("%d %d %d %d %d %d %d\n", a1, a2, a3, a4, a5, a6, a7);
-}
+//init ros advertise and subscribe
+//    nh.initNode();
+//    nh.advertise(statePub);
+//    nh.subscribe(commandSub);
 
-Thread thread;
-
-DigitalOut led1(LED1);
+    kicker.period(0.01f);
+    kicker = 1;   //deactivate kicker, active LOW
 
-void print_thread()
-{
+    thread1.start(mainProcess);
+//    thread2.start(getCompass);
+
     while (true) {
-        wait(1);
-        print_char();
+        //do nothing
     }
 }
 
-int main()
-{
-    printf("\n\n*** RTOS basic example ***\n");
+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;
 
-    thread.start(print_thread);
+        //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;
+//        } 
 
-    while (true) {
-        led1 = !led1;
-        wait(0.5);
+//        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;
+}
\ No newline at end of file