Dependencies: cmps_dagoz motor_dagoz encoder_dagoz ros_lib_kinetic EncoderMotorInterrupt
Revision 1:383eeb6f9e45, committed 2019-02-09
- Comitter:
- calmantara186
- Date:
- Sat Feb 09 08:12:34 2019 +0000
- Parent:
- 0:1c22457d4aed
- Commit message:
- Initial Commit
Changed in this revision
diff -r 1c22457d4aed -r 383eeb6f9e45 Constanta.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Constanta.h Sat Feb 09 08:12:34 2019 +0000 @@ -0,0 +1,59 @@ +#ifndef CONSTANTA_H +#define CONSTANTA_H + +#include "mbed.h" +/***************************** + CONSTANTS + *****************************/ +// Robot Components +const double PI = 3.141593; //PI constanta +const double LOCOMOTIONWHEEL = 0.03; //Metres +const double DRIBBLERWHEEL = 0.035; //Metres +// Locomotion Components +const float KVFF = 1.0; //Velocity Feedforward for Locomotion +const float KAFF = 1.0; //Acceleration Feedforward for Locomotion +const float ACCT = 2.0; //Acceleration for robot Translation +const float ACCR = 2.0; //Acceleration for robot rotation +// Right Dribbler Components +const float RK1FB = 1.0; //Right Dribbler Pot to Omega feedback +const float RK2FB = 1.0; //Right Dribbler Error to pwm feedback +const float RKVFF = 1.0; //Right Dribbler velocity feedforward +const float RKXFF = 1.0; //Right Dribbler x velocity feedforward +const float RKYFF = 1.0; //Right Dribbler y velocity feedforward +const float RKTFF = 1.0; //Right Dribbler w velocity feedforward +const float RTARGET = 1.0; //Right Dribbler potensio target +// Left Dribbler Components +const float LK1FB = 1.0; //Left Dribbler Pot to Omega feedback +const float LK2FB = 1.0; //Left Dribbler Error to pwm feedback +const float LKVFF = 1.0; //Left Dribbler velocity feedforward +const float LKXFF = 1.0; //Left Dribbler x velocity feedforward +const float LKYFF = 1.0; //Left Dribbler y velocity feedforward +const float LKTFF = 1.0; //Left Dribbler w velocity feedforward +const float LTARGET = 1.0; //Left Dribbler potensio target +/***************************** + Global Variable + *****************************/ +//Compass Global Varible +float compassValue = 0; //Compass value +//Odometry global variable +double locomotionRVel = 0; +double locomotionLVel = 0; +double locomotionBVel = 0; +double locomotionRRot = 0; +double locomotionLRot = 0; +double locomotionBRot = 0; +double locomotionRVtarget = 0; +double locomotionLVtarget = 0; +double locomotionBVtarget = 0; +//Dribbler global variabel +double dribblerRVel = 0; +double dribblerLVel = 0; +double dribblerRRot = 0; +double dribblerLRot = 0; +double dribblerRVtarget = 0; +double dribblerLVtarget = 0; +//Kicker global variable +double kickPowerTarget = 0; +float distance = 0; + +#endif \ No newline at end of file
diff -r 1c22457d4aed -r 383eeb6f9e45 EncoderMotorInterrupt.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EncoderMotorInterrupt.lib Sat Feb 09 08:12:34 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Spirit-Dagozilla/code/EncoderMotorInterrupt/#115067a48593
diff -r 1c22457d4aed -r 383eeb6f9e45 Encoder_Nucleo_32_bits.lib --- a/Encoder_Nucleo_32_bits.lib Thu Jan 17 05:22:45 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/teams/Dagozilla-to-RoboCup/code/Encoder_Nucleo_32_bits/#a3349f37ef99
diff -r 1c22457d4aed -r 383eeb6f9e45 PIDDagoz.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PIDDagoz.lib Sat Feb 09 08:12:34 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Spirit-Dagozilla/code/PIDDagoz1/#9617685ae239
diff -r 1c22457d4aed -r 383eeb6f9e45 cmps_dagoz.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/cmps_dagoz.lib Sat Feb 09 08:12:34 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Spirit-Dagozilla/code/cmps_dagoz/#9619d0f34a61
diff -r 1c22457d4aed -r 383eeb6f9e45 encoder_dagoz.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/encoder_dagoz.lib Sat Feb 09 08:12:34 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Spirit-Dagozilla/code/encoder_dagoz/#30d4e9abf3ac
diff -r 1c22457d4aed -r 383eeb6f9e45 main.cpp
--- a/main.cpp Thu Jan 17 05:22:45 2019 +0000
+++ b/main.cpp Sat Feb 09 08:12:34 2019 +0000
@@ -1,46 +1,212 @@
#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(PA_11, PB_12, 537.6, EncoderMotor::X4_ENCODING);
+EncoderMotor intEncR(PB_13, PC_4, 537.6, EncoderMotor::X4_ENCODING);
+EncoderMotor intEncB(PC_11, PD_2, 537.6, EncoderMotor::X4_ENCODING);
+
+//Motor pin buat Board Sistem next ver.
+MotorDagoz locomotionMotorR(PH_1, PB_8); //Locomotion Right Motor
+MotorDagoz locomotionMotorL(PA_12, PB_9); //Locomotion Left Motor
+MotorDagoz locomotionMotorB(PA_10, PA_6); //Locomotion Back Motor
+
+MotorDagoz dribblerMotorR(PA_11, PB_14); //Dribbler Right Motor
+MotorDagoz dribblerMotorL(PB_10, PB_15); //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 dribblerPotL(PC_0); //Potensio for Left Dribbler
+AnalogIn dribblerPotR(PC_1); //Potensio for Right Dribbler
+AnalogIn kickerPot(PC_2); //Potensio for Kicker
+
+//LED Pin
+DigitalOut ledRed(PA_4); //Red LED
+DigitalOut ledGreen(PC_10); //Green LED
+DigitalOut ledBlue(PC_12); //Blue LED
+
+//Infrared Pin
+AnalogIn infraRed(PC_3); //Ball distance IR
+
+//Kicker PWM Pin
+PwmOut kicker(PA_7); //Kicker pwm effort
+
+////Stepper Pin
+DigitalOut stepperKickerDir(PA_13); //Direction pin for kicker stepper motor
+DigitalOut stepperKickerStep(PA_14); //Step pin for kicker stepper motor
+
+DigitalOut stepperExpVerDir(PC_13); //Direction pin for vertical expanding mechanism stepper motor
+DigitalOut stepperExpVerStep(PC_14); //Step pin for vertical expanding mechanism stepper motor
-void print_char()
+DigitalOut stepperExpHorDir(PC_15); //Direction pin for horizontal expanding mechanism stepper motor
+DigitalOut stepperExpHorStep(PH_0); //Step pin for horizontal expanding mechanism stepper motor
+
+/******************************
+ 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.22210959642179, 5.95044130151099, 0, 100, 0.02); //Right locomotion PID
+PID locomotionPidL(0.208433282254394, 6.80238393068066, 0, 100, 0.02); //Left locomotion PID
+PID locomotionPidB(0.209120668853316, 6.43233135464926, 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
+
+/*****************************
+ 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);
+
+ kicker.period(0.01f);
+ kicker = 1; //deactivate kicker, active LOW
+
+ //encoder value
+ int curLocomotionR = 0;
+ int curLocomotionL = 0;
+ int curLocomotionB = 0;
+ int curDribblerR = 0;
+ int curDribblerL = 0;
+ //Potensio value
+ float curPotL = 0;
+ float curPotR = 0;
+ //feedforward transition
+ double targetVelL = 0;
+ double targetVelR = 0;
+ double prevTargetVelL = 0;
+ double prevTargetVelR = 0;
+ //pwm value
+ double locomotionRTargetRate = 0;
+ double locomotionLTargetRate = 0;
+ double locomotionBTargetRate = 0;
+ double dribblerLTargetRate = 0;
+ double dribblerRTargetRate = 0;
-Thread thread;
-
-DigitalOut led1(LED1);
+ while (true) {
+ //do nothing
+// nh.spinOnce();
+// curPotL = ((float)dribblerPotL.read()/3.3)*(2*PI);
+// curPotR = ((float)dribblerPotR.read()/3.3)*(2*PI);
+// //ball distance from IR
+// distance = infraRed.read();
+// //read encoder
+// curLocomotionR = -locomotionEncR.GetCounter(1)/4000.0;
+// curLocomotionL = -locomotionEncL.GetCounter(1)/4000.0;
+// curLocomotionB = -locomotionEncB.GetCounter(1)/4000.0;
+// curDribblerR = dribblerEncR.GetCounter(1)/537.6;
+// curDribblerL = dribblerEncL.GetCounter(1)/537.6;
+// // Correct encoder unit
+// locomotionRRot = curLocomotionR * LOCOMOTIONWHEEL * 2 * PI; // in m
+// locomotionLRot = curLocomotionL * LOCOMOTIONWHEEL * 2 * PI; // in m
+// locomotionBRot = curLocomotionB * LOCOMOTIONWHEEL * 2 * PI; // in m
+// dribblerRRot = curDribblerL * DRIBBLERWHEEL * 2 * PI; // in m
+// dribblerLRot = curDribblerR * DRIBBLERWHEEL * 2 * PI; // in m
+// // Calculate acutal velocity
+// locomotionRVel = locomotionRRot / 0.002;
+// locomotionLVel = locomotionLRot / 0.002;
+// locomotionBVel = locomotionBRot / 0.002;
+// dribblerRVel = dribblerRRot / 0.002;
+// dribblerLVel = dribblerLRot / 0.002;
+// // Calculate motor pwm
+// locomotionRTargetRate = locomotionPidR.createpwm(locomotionRVtarget, locomotionRVel);
+// locomotionLTargetRate = locomotionPidL.createpwm(locomotionLVtarget, locomotionLVel);
+// locomotionBTargetRate = locomotionPidB.createpwm(locomotionBVtarget, locomotionBVel);
+// dribblerLTargetRate = dribblerPidL.createpwm(dribblerLVtarget, dribblerLVel);
+// dribblerRTargetRate = dribblerPidR.createpwm(dribblerRVtarget, dribblerRVel);
+// //Calculate Feedback and FeedForward
+// //need correction for +- in each robot velocity
+// targetVelL = ((LTARGET - curPotL)*LK1FB)+(velX*LKXFF + velY*LKYFF + velW*LKTFF);
+// targetVelR = ((RTARGET - curPotR)*RK1FB)+(velX*RKXFF + velY*RKYFF + velW*RKTFF);
+// dribblerLTargetRate = ((targetVelL - dribblerLVel)*LK2FB)+(LKVFF*(targetVelL-prevTargetVelL));
+// dribblerRTargetRate = ((targetVelR - dribblerRVel)*RK2FB)+(RKVFF*(targetVelR-prevTargetVelR));
+// prevTargetVelL = targetVelL;
+// prevTargetVelR = targetVelR;
+// //Motor pwm
+// locomotionMotorR.setpwm(locomotionRTargetRate);
+// locomotionMotorL.setpwm(locomotionLTargetRate);
+// locomotionMotorB.setpwm(locomotionBTargetRate);
+// dribblerMotorL.setpwm(0.2);
+// dribblerMotorR.setpwm(0.2);
+// //kicker conditional
+// if(kickPowerTarget > 0) {
+// kicker.write(1-kickPowerTarget); //Active LOW
+// Thread::wait(15);
+// kicker = 1;
+// kickPowerTarget = 0;
+// }
+// Thread::wait(5);
+// //Publish position data
+// stateMsg.base_wheel_right_position = locomotionRRot; // in m
+// stateMsg.base_wheel_back_position = locomotionBRot; // in m
+// stateMsg.base_wheel_left_position = locomotionLRot; // in m
+// stateMsg.dribbler_wheel_left_position = dribblerLRot; // in m
+// stateMsg.dribbler_wheel_right_position = dribblerRRot; // in m
+// //Publish velocity data
+// stateMsg.base_wheel_right_velocity = locomotionRVel; // in m/s
+// stateMsg.base_wheel_back_velocity = locomotionBVel; // in m/s
+// stateMsg.base_wheel_left_velocity = locomotionLVel; // in m/s
+// stateMsg.dribbler_wheel_left_velocity = dribblerLVel; // in m/s
+// stateMsg.dribbler_wheel_right_velocity = dribblerRVel; // in m/s
+// //Publish distance data
+// stateMsg.dribbler_ir_distance = distance; // in analog value
+// statePub.publish(&stateMsg);
+// nh.spinOnce();
-void print_thread()
-{
- while (true) {
- wait(1);
- print_char();
+ curLocomotionR = -locomotionEncR.GetCounter(0);
+ curLocomotionL = -locomotionEncL.GetCounter(0);
+ curLocomotionB = -locomotionEncB.GetCounter(0);
+ curDribblerR = dribblerEncR.GetCounter(0);
+ curDribblerL = dribblerEncL.GetCounter(0);
+
+ pc.printf("%d, %d, %d, %d, %d\n", curLocomotionR, curLocomotionL, curLocomotionB, curDribblerR, curDribblerL);
+ Thread::wait(15);
}
}
-int main()
-{
- printf("\n\n*** RTOS basic example ***\n");
-
- thread.start(print_thread);
-
- while (true) {
- led1 = !led1;
- wait(0.5);
- }
-}
+//void commandCallback(const dagozilla_msgs::HardwareCommandMsg& commandMsg) {
+// locomotionRVtarget = commandMsg.base_wheel_right_velocity;
+// locomotionBVtarget = commandMsg.base_wheel_back_velocity;
+// locomotionLVtarget = commandMsg.base_wheel_left_velocity;
+// dribblerLVtarget = commandMsg.dribbler_wheel_left_velocity;
+// dribblerRVtarget = commandMsg.dribbler_wheel_left_velocity;
+// kickPowerTarget = commandMsg.kicker_effort;
+//}
\ No newline at end of file
diff -r 1c22457d4aed -r 383eeb6f9e45 motor_dagoz.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor_dagoz.lib Sat Feb 09 08:12:34 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Spirit-Dagozilla/code/motor_dagoz/#eec0fee18a92
diff -r 1c22457d4aed -r 383eeb6f9e45 ros_lib_kinetic.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Sat Feb 09 08:12:34 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f