Spirit Dagozilla
buat ambil data dribbler
Dependencies: cmps_dagoz motor_dagoz encoder_dagoz ros_lib_kinetic EncoderMotorInterrupt
Revision 1:3cc942422d2f, committed 2019-02-09
- Comitter:
- calmantara186
- Date:
- Sat Feb 09 13:27:40 2019 +0000
- Parent:
- 0:1c22457d4aed
- Commit message:
- initial commit
Changed in this revision
diff -r 1c22457d4aed -r 3cc942422d2f Constanta.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Constanta.h Sat Feb 09 13:27:40 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.05; //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 3cc942422d2f EncoderMotorInterrupt.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EncoderMotorInterrupt.lib Sat Feb 09 13:27:40 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Spirit-Dagozilla/code/EncoderMotorInterrupt/#115067a48593
diff -r 1c22457d4aed -r 3cc942422d2f 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 3cc942422d2f PIDDagoz.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PIDDagoz.lib Sat Feb 09 13:27:40 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Spirit-Dagozilla/code/PIDDagoz1/#9617685ae239
diff -r 1c22457d4aed -r 3cc942422d2f cmps_dagoz.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/cmps_dagoz.lib Sat Feb 09 13:27:40 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Spirit-Dagozilla/code/cmps_dagoz/#58cfc8b5c768
diff -r 1c22457d4aed -r 3cc942422d2f encoder_dagoz.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/encoder_dagoz.lib Sat Feb 09 13:27:40 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Spirit-Dagozilla/code/encoder_dagoz/#d7c793ec5c04
diff -r 1c22457d4aed -r 3cc942422d2f main.cpp
--- a/main.cpp Thu Jan 17 05:22:45 2019 +0000
+++ b/main.cpp Sat Feb 09 13:27:40 2019 +0000
@@ -1,46 +1,65 @@
#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"
-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);
+/*****************************
+ Pin Declaration
+ *****************************/
+//Encoder pin
+EncoderDAGOZ locomotionEncR(TIM1); //Locomotion Right Encoder
+EncoderDAGOZ locomotionEncL(TIM2); //Locomotion Left Encoder
+EncoderDAGOZ locomotionEncB(TIM3); //Locomotion Back Encoder
+EncoderDAGOZ dribblerEncR(TIM4); //Dribbler Right Encoder
+EncoderDAGOZ dribblerEncL(TIM8); //Dribbler Left Encoder
-void print_char()
-{
- 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);
-}
+//Motor Pins
+MotorDagoz locomotionMotorR(PF_12, PF_8); //Locomotion Right Motor
+MotorDagoz locomotionMotorL(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
-Thread thread;
-
-DigitalOut led1(LED1);
+//Serial pin
+Serial pc(USBTX, USBRX, 9600); //Serial debug
-void print_thread()
-{
- while (true) {
- wait(1);
- print_char();
- }
-}
+//Velocity Data Array
+double vDribblerL[1000], vDribblerR[1000];
+
+//Sampling Time
+double Ts = 0.005; //ms
+double r = 0.05; //m
+double Pi = 3.14159265359;
int main()
{
- printf("\n\n*** RTOS basic example ***\n");
-
- thread.start(print_thread);
-
+ wait(10);
+
+ pc.printf("vDribblerL,vDribblerR\n");
+
+ for (int i = 0; i < 1000; i++){
+
+ vDribblerL[i] = dribblerEncL.GetCounter(1) * 2 * Pi * 0.03125 / (537.6 * Ts);
+ vDribblerR[i] = dribblerEncR.GetCounter(1) * 2 * Pi * 0.03125 / (537.6 * Ts);
+
+ dribblerMotorL.setpwm(1.0);
+ dribblerMotorR.setpwm(1.0);
+
+ wait_ms(Ts*1000);
+ }
+
+ dribblerMotorL.setpwm(0.0);
+ dribblerMotorR.setpwm(0.0);
+
+ for (int i = 0; i < 1000; i++){
+ pc.printf("%d,%d\n\r", vDribblerL[i]*100000, vDribblerR[i]*100000);
+ }
+
while (true) {
- led1 = !led1;
- wait(0.5);
+ //do nothing
}
-}
+}
\ No newline at end of file
diff -r 1c22457d4aed -r 3cc942422d2f motor_dagoz.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor_dagoz.lib Sat Feb 09 13:27:40 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Spirit-Dagozilla/code/motor_dagoz/#6551c3b89324
diff -r 1c22457d4aed -r 3cc942422d2f ros_lib_kinetic.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Sat Feb 09 13:27:40 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f