Revision 0:52f6905317cb, committed 2021-06-17

Comitter:

ftppr

Date:

Thu Jun 17 17:37:18 2021 +0000

Commit message:

use stupid method

Changed in this revision

diff -r 000000000000 -r 52f6905317cb DC_Stepper_Controller_Lib.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DC_Stepper_Controller_Lib.lib	Thu Jun 17 17:37:18 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/Robocon-2021/code/DC_Stepper_Controller_Lib/#c9b18083021f

diff -r 000000000000 -r 52f6905317cb main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jun 17 17:37:18 2021 +0000
@@ -0,0 +1,106 @@
+#include "mbed.h"
+#include "DC_Motor_Controller.h"
+
+DC_Motor_PID dc_motor2(D10, D12, PC_8, PC_9, 792); //M1, M2, INA, INB, PPR; picker // D11, D10 see works?  original: D4, D7 for encoder, D12, D13 for out
+
+Serial myPC(USBTX, USBRX);  // import serial for the UART attached toUSB
+
+DigitalOut myLED(LED1);
+DigitalIn button(USER_BUTTON);
+DigitalIn firing3 = D7; // Pick, short
+DigitalIn firing4 = D4; // Pick, long
+DigitalIn firing5 = D8;
+DigitalOut faul2_1 = A1;
+DigitalOut faul2_2 = A2;
+ DigitalOut valve_1 = D0; // valve 1
+ DigitalOut valve_2 = D1; // valve 2
+
+int main()
+{
+    firing3.mode(PullUp); // default 0
+    firing4.mode(PullUp); // default 0
+    firing5.mode(PullUp); // default 0
+    int stage_pick = 0;
+    dc_motor2.reset();
+    dc_motor2.set_pid(0.008, 0, 0);
+
+    myPC.printf("Initialization Finished \n\r");
+    myPC.printf("Firing3 is %d, Firing4 is %d, button is %d. \n\r", firing3.read(), firing4.read(), button.read()); // valve omitted
+    while(1)
+    {
+        if(button == 1)
+        {
+            if(stage_pick == 0)
+            {
+                myPC.printf("dc_motor reset\r\n");
+                dc_motor2.set_out(0.5, 0);
+                while(firing3 == 0);
+                if(firing3 == 1)
+                {
+                    dc_motor2.set_out(0,0);
+                    myPC.printf("short: firing3 pin detected\n\r");
+                }
+                stage_pick++;
+                valve_1 = 1; valve_2 = 0;
+            }
+            else if(stage_pick == 1)
+            {
+                myPC.printf("faulhaber 2 RefPos\n\r");
+                faul2_1 = 1;
+                while(firing4 == 0);
+                faul2_1 = 0;  
+                myPC.printf("long: firing4 detected \n\r"); //內
+                stage_pick++;              
+            }
+            else if(stage_pick == 2)
+            {
+                myPC.printf("dc_motor rotate to pick up\n\r");
+                dc_motor2.set_out(0,0.5);
+                wait(0.3);
+                dc_motor2.set_out(0,0);
+                stage_pick++;
+                
+            }
+            else if(stage_pick == 3)
+            {
+                myPC.printf("faul2 to pick the arrows \n\r"); // 外
+                faul2_2 = 1;
+                while(firing5 == 0);
+                wait(0.3);
+                faul2_2 = 0;
+                myPC.printf("Firing5 touched \n\r"); 
+                
+                stage_pick++; // use firing 5
+            }
+            else if(stage_pick == 4)
+            {                
+                myPC.printf("faul2 rotate to vertical \n\r"); // 內一啲
+                faul2_1 = 1;
+                wait(2.6);
+                faul2_1 = 0;
+                stage_pick++;
+            }
+            else if(stage_pick == 5)
+            {
+                myPC.printf("dc_motor rotate to final pos\n\r");
+                dc_motor2.set_out(0,1);
+                wait(0.4);
+                dc_motor2.set_out(0,0);
+                stage_pick++;
+            }
+            else if(stage_pick == 6)
+            {
+                myPC.printf("faul2 rotate to final pos \n\r"); // 內一啲
+                faul2_1 = 1;
+                wait(1.9);
+                faul2_1 = 0;
+                stage_pick = 0;
+            }
+            while(button == 1);
+            wait(0.3);
+        }
+        
+    }
+    
+        
+}

diff -r 000000000000 -r 52f6905317cb main_copy.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_copy.cpp	Thu Jun 17 17:37:18 2021 +0000
@@ -0,0 +1,179 @@
+/*#include "mbed.h"
+#include "DC_Motor_Controller.h"
+
+//DigitalIn home_button(D13);     // Button for setting the home button
+DC_Motor_PID dc_motor_1(D7, D6, D10, D11, 792); //M1, M2, INA, INB, PPR
+DC_Motor_PID dc_motor_2(D4, D5, D9, D8, 792); //M1, M2, INA, INB, PPR
+Serial myPC(USBTX, USBRX);  // import serial for the UART attached toUSB
+// DigitalOut control_1_1(D7); // control pin on L298N - 1
+// DigitalOut control_1_2(D6); // control pin on L298N - 2
+// DigitalOut control_2_1(D4); // control pin on L298N - 1
+// DigitalOut control_2_2(D5); // control pin on L298N - 2
+// PwmOut speed(D6);           // pwm signal for
+DigitalOut myLED(LED1);
+DigitalIn button(USER_BUTTON);
+// DigitalIn valve(D9, PullDown);
+DigitalIn firing1(D2);
+DigitalIn firing2(D3);
+// DigitalOut valve_1 = D12; // valve 1
+// DigitalOut valve_2 = D13; // valve 2
+void reset(DigitalIn firing_pin, DC_Motor_PID& dc_motor, int forward)
+{
+    if (forward)
+    {
+        dc_motor.set_out(1, 0);
+    }
+    else
+    {
+        dc_motor.set_out(0, 1);
+    }
+
+    myPC.printf("Enter reset, \r\n");
+    while (1)
+    {
+        if (firing_pin == 0)
+        {
+            myPC.printf("Firing switch detected. Stop. Record reference point %d.\r\n", dc_motor.get_pulse());
+            dc_motor.reset();
+            dc_motor.set_out(0, 0);
+            while (firing_pin == 0)
+                ;
+            return;
+        }
+    }
+};
+float pulse_to_angle(int pulses)
+{
+    return pulses * 360 / 2 / 792;
+}
+float angle_to_pulse(int angle)
+{
+    return angle / 360 * 2 * 792;
+}
+int main()
+{
+    // DC_Motor_PID dc_motor_1(D7, D6, D10, D11, 792); //M1, M2, INA, INB, PPR
+    // DC_Motor_PID dc_motor_2(D4, D5, D9, D8, 792);   //M1, M2, INA, INB, PPR
+    dc_motor_1.reset();
+    dc_motor_2.reset();
+    // initialize pwm
+    // speed.period(0.01);
+    // speed = 0.9f;
+    // initialize control pins on L298N
+    dc_motor_1.reset();
+    dc_motor_2.reset();
+    myPC.printf("Initialization Finished \n\r");
+    myPC.printf("Firing1 is %d, Firing2 is %d, button is %d. \n\r", firing1.read(), firing2.read(), button.read()); // valve omitted
+    // open catcher
+    // valve_1 = 1;
+    // valve_2 = 0; // tentative
+    // myPC.printf("Catcher opened. valves are %d%d. \n\r", valve_1, valve_2);
+
+    // Variables
+    int ref_pick_1 = 0;
+    int ref_pick_2 = 0;
+    int angle_long_1 = 0;
+    int angle_short_1 = 0; // angles used in phase 1
+    int angle_long_2 = 0;
+    int angle_short_2 = 0; // angles used in phase 2
+    int stage = 0;         // stages for picking arrow
+    // Press the user button to start initialization
+    while (1)
+    {
+        if (button == 1)
+        {
+            while (button == 1)
+                ;
+            break;
+        }
+    }
+
+    myPC.printf("=========== Start configuring the long motor ==========\r\n");
+    // myPC.printf("motor1 starts to rotate until firing pin is reached.\r\n");
+    reset(firing1, dc_motor_1, 1);
+    myPC.printf("Motor 1 reset finished. Press button to reset Motor 2.\r\n");
+    // if (firing1 == 1)
+    // {
+    //     myPC.printf("Firing1 switch detected. Stop. Record reference point_1  %d.\r\n", dc_motor_1.get_pulse());
+    //     control_1_1 = 0;
+    //     control_1_2 = 0;
+    //     ref_pick_1 = dc_motor_1.get_pulse(); // stop -> record or reverse????
+    //     while (firing1 == 1)
+    //         ;
+    // }
+    while (button == 0)
+        ; // idle
+    while (button == 1)
+        ; // button pressed
+    myPC.printf("=========== Start configuring the short motor ==========\r\n");
+    // myPC.printf("motor1 starts to rotate until firing pin is reached.\r\n");
+    reset(firing2, dc_motor_2, 1);
+
+    // if (firing2 == 1)
+    // {
+    //     myPC.printf("firing2 switch detected. Stop. Record reference point_2  %d.\r\n", dc_motor_2.get_pulse());
+    //     control_2_1 = 0;
+    //     control_2_2 = 0;
+    //     ref_pick_2 = dc_motor_2.get_pulse(); // stop -> record or reverse????
+    //     while (firing2 == 1)
+    //         ;
+    // }
+
+    myPC.printf("=========== Initialization finished. Press user button to pick up arrows ==========\r\n");
+
+    while (1)
+    {
+        if (button == 1)
+        {
+            myPC.printf("Start picking up arrows\r\n");
+            while (button == 1)
+                ;
+            if (stage == 0)
+            {
+                myPC.printf("Begin stage 1\r\n");
+                myPC.printf("Rotating long one for % degree. \r\n", angle_long_1);
+                myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_1.get_pulse()), pulse_to_angle(dc_motor_1.get_pulse()) + angle_long_1);
+                dc_motor_1.move_angle(angle_long_1);
+                myPC.printf("Rotating short one for % degree. \r\n", angle_short_1);
+                myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_2.get_pulse()), pulse_to_angle(dc_motor_2.get_pulse()) + angle_short_1);
+                dc_motor_2.move_angle(angle_short_1);
+                stage++;
+                myPC.printf("Finished stage 1\r\n");
+            }
+            else if (stage == 1)
+            {
+                myPC.printf("Begin stage 2\r\n");
+                myPC.printf("Rotating long one for % degree. \r\n", angle_long_2);
+                myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_1.get_pulse()), pulse_to_angle(dc_motor_1.get_pulse()) + angle_long_2);
+                dc_motor_1.move_angle(angle_long_2);
+                myPC.printf("Rotating short one for % degree. \r\n", angle_short_1);
+                myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_2.get_pulse()), pulse_to_angle(dc_motor_2.get_pulse()) + angle_short_2);
+                dc_motor_2.move_angle(angle_short_2);
+                stage++;
+                myPC.printf("Finished stage 2\r\n");
+            }
+        }
+
+        // another button for controlling the valve
+        // if(valve == 0)
+        // {
+
+        //     valve_1 = 1 - valve_1;
+        //     valve_2 = 1 - valve_2;
+        //     myPC.printf("Change catcher state. %d%d.\r\n", valve_1, valve_2);
+        //     while (valve)
+        //         ;
+        // }
+        // if finished one process, reset
+        if (stage > 1)
+        {
+            myPC.printf("=========== Start resetting the long motor ==========\r\n");
+            reset(firing1, dc_motor_1, 0); // forward = 0 -> rotate backward
+            myPC.printf("Motor 1 reset finished. Press button to reset Motor 2.\r\n");
+            myPC.printf("=========== Start resetting the short motor ==========\r\n");
+            reset(firing2, dc_motor_2, 0);
+            myPC.printf("=========== Resetting finished. Press user button to pick up arrows ==========\r\n");
+            stage = 0;
+        }
+    }
+}*/

diff -r 000000000000 -r 52f6905317cb mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Jun 17 17:37:18 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

diff -r 000000000000 -r 52f6905317cb ros_lib_melodic.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_melodic.lib	Thu Jun 17 17:37:18 2021 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/garyservin/code/ros_lib_melodic/#da82487f547e