Diff: main.cpp

Revision:

3:0521855e7337

Parent:

2:830d3c808679

--- a/main.cpp	Sat Mar 23 09:48:27 2019 +0000
+++ b/main.cpp	Sat Feb 29 10:27:57 2020 +0000
@@ -1,107 +1,54 @@
 #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>
+#include <ros.h>
+#include <dagozilla_utils/config.h>
+#include <dagozilla_utils/robotpin.h>
+#include <dagozilla_utils/variable.h>
+#include <dagozilla_msgs/HardwareCommand.h>
+#include <dagozilla_msgs/HardwareState.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
+ros::NodeHandle nh;
 
 /******************************
   Publisher-Subscriber
- ******************************/
-// dagozilla_msgs::HardwareStateMsg stateMsg;
-// ros::Publisher statePub("/hardware/state", &stateMsg);
+******************************/
+dagozilla_msgs::HardwareState stateMsg;
+ros::Publisher statePub("/nucleo/state/hardware", &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 
+void commandCallback(const dagozilla_msgs::HardwareCommand& commandMsg);
+ros::Subscriber<dagozilla_msgs::HardwareCommand> commandSub("/control/command/hardware", &commandCallback);
 
 //thread for RTOS
-Thread thread1;
-Thread thread2;
+Thread thread1(osPriorityNormal);
+Thread thread2(osPriorityAboveNormal);
 
 //primitive function
 void mainProcess();
 void getCompass();
 void moveLocomotion();
+void publishMessage();
 void moveDribbler();
-void publishMessage();
-
-bool dribbler_state;
 
 /*****************************
- Main Function
+  Main Function
  *****************************/
 
 int main()
 {
-//init ros advertise and subscribe
-    // nh.initNode();
-    // nh.advertise(statePub);
-    // nh.subscribe(commandSub);
-
+    //init ros advertise and subscribe
+    nh.getHardware()->setBaud(115200);
+    nh.initNode();
+    nh.advertise(statePub);
+    nh.subscribe(commandSub);
+    t.start();
+    
     kicker.period(0.01f);
     kicker = 1;   //deactivate kicker, active LOW
 
     thread1.start(mainProcess);
-    thread2.start(getCompass);
+//    thread2.start(getCompass);
 
     while (true) {
         //do nothing
@@ -109,54 +56,53 @@
 }
 
 void mainProcess(){
-    float cur_pot_L0 = ((float)dribblerPotL.read()*100.0f);
-    float cur_pot_R0 = ((float)dribblerPotR.read()*100.0f);
+    float cur_pot_L0 = (float)dribblerPotL.read() * SCALE_POT_L;
+    float cur_pot_R0 = (float)dribblerPotR.read() * SCALE_POT_R;
     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);
+        cur_pot_L = (float)dribblerPotL.read() * SCALE_POT_L - cur_pot_L0;
+        cur_pot_R = (float)dribblerPotR.read() * SCALE_POT_R - cur_pot_R0;
+                
+        nh.spinOnce();
+        
         //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;
+        cur_locomotion_R = -locomotionEncR.GetCounter(1);
+        cur_locomotion_L = -locomotionEncL.GetCounter(1);
+        
+        rotInFL = intEncFL.getPulses(1);
+        rotInFR = intEncFR.getPulses(1);
+        rotInBL = intEncBL.getPulses(1);
+        rotInBR = intEncBR.getPulses(1);
         
-        rotInB = intEncR.getRevolutions();
-        rotInR = intEncL.getRevolutions();
-        rotInL = intEncB.getRevolutions();
+        cur_dribbler_L = -dribblerEncL.GetCounter(1);
+        cur_dribbler_R = -dribblerEncR.GetCounter(1);
+        
+        moveLocomotion();
+        moveDribbler();
+        
+        kicker.write(1-kick_power_target);
         
-        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;
+        publishMessage();
+        nh.spinOnce();
         
-//        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
+        if (t - last_timer >=1000){
+            
+            locomotion_FL_target_rate = 0;
+            locomotion_FR_target_rate = 0;
+            locomotion_BL_target_rate = 0;
+            locomotion_BR_target_rate = 0;
+            
+            dribbler_L_target_rate = 0;
+            dribbler_R_target_rate = 0;
+            
+            kick_power_target = 1;
+            kicker_shoot_mode = 0;
+        }
         
-        // publishMessage();
-        // nh.spinOnce();   
         Thread::wait(20);
     }
 }
@@ -168,108 +114,70 @@
     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;    
+            theta_com = (theta_temp - 360.0)/-RADTODEG;    
         else if(theta_temp < -180.0 && theta_temp >= -360.0)
-            theta_com = ((compass.readBearing()/10.0) + 360.0 - theta0)/-RADTODEG;
+            theta_com = (theta_temp + 360.0)/-RADTODEG;
         else
-            theta_com = ((compass.readBearing()/10.0) - theta0)/-RADTODEG;
-        
-        if(theta != 0) compassLed = 1;
-        else compassLed = 0;
+            theta_com = theta_temp/-RADTODEG;
         
         theta = theta_com;
 
-        if(theta != 0) compassLed = 1;
+        if(fabs(theta) >= 10.0/RADTODEG) compassLed = 1;
         else compassLed = 0;
-
-        Thread::wait(60);
+        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);
-    
-    //Motor pwm    
-    locomotionMotorR.setpwm(locomotion_R_target_rate);
-    locomotionMotorL.setpwm(locomotion_L_target_rate);
-    locomotionMotorB.setpwm(locomotion_B_target_rate);
+    locomotionMotorFL.setpwm(locomotion_FL_target_rate);
+    locomotionMotorFR.setpwm(locomotion_FR_target_rate);
+    locomotionMotorBL.setpwm(locomotion_BL_target_rate);
+    locomotionMotorBR.setpwm(locomotion_BR_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); 
-    
+    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 publishMessage(){
 
-// 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;
-// }
\ No newline at end of file
+    // Publish position data
+    stateMsg.base_motor_1_pulses = rotInFL;
+    stateMsg.base_motor_2_pulses = rotInFR; 
+    stateMsg.base_motor_3_pulses = rotInBL;
+    stateMsg.base_motor_4_pulses = rotInBR;
+    
+    stateMsg.base_encoder_1_pulses = cur_locomotion_L;
+    stateMsg.base_encoder_2_pulses = cur_locomotion_R;
+    stateMsg.base_encoder_3_pulses = 0;
+    
+    stateMsg.dribbler_motor_l_pulses = cur_dribbler_L;
+    stateMsg.dribbler_motor_r_pulses = cur_dribbler_R;
+    
+    stateMsg.dribbler_potentio_l_reading = cur_pot_L;
+    stateMsg.dribbler_potentio_r_reading = cur_pot_R;
+    
+    stateMsg.ir_reading = distance;
+    stateMsg.compass_reading = theta;
+    
+    statePub.publish(&stateMsg);
+}
+
+void commandCallback(const dagozilla_msgs::HardwareCommand& commandMsg) {
+      
+    locomotion_FL_target_rate = commandMsg.base_motor_1_pwm;
+    locomotion_FR_target_rate = commandMsg.base_motor_2_pwm;
+    locomotion_BL_target_rate = commandMsg.base_motor_3_pwm;
+    locomotion_BR_target_rate = commandMsg.base_motor_4_pwm;
+    
+    dribbler_L_target_rate = commandMsg.dribbler_motor_l_pwm;
+    dribbler_R_target_rate = commandMsg.dribbler_motor_r_pwm;
+    
+    kick_power_target = commandMsg.kicker_pwm;
+    kicker_shoot_mode = commandMsg.kicker_mode;
+    
+    last_timer = t;
+}