Revision 32:481674c0f90d, committed 2018-04-23

Comitter:

Brunoporto2702

Date:

Mon Apr 23 10:55:24 2018 +0000

Parent:

31:f203b17d5534

Child:

33:b276c9c00088

Commit message:

2 Motores + joystick

Changed in this revision

--- a/main.cpp	Fri Jul 14 15:33:04 2017 +0000
+++ b/main.cpp	Mon Apr 23 10:55:24 2018 +0000
@@ -66,7 +66,54 @@
 #define SPEED_2 1200
 
 
+
 /* Variables -----------------------------------------------------------------*/
+/* Initialization parameters. */
+L6474_init_t init = {
+    160,                              /* Acceleration rate in pps^2. Range: (0..+inf). */
+    160,                              /* Deceleration rate in pps^2. Range: (0..+inf). */
+    2400,                             /* Maximum speed in pps. Range: (30..10000]. */
+    30,                              /* Minimum speed in pps. Range: [30..10000). */
+    250,                              /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
+    L6474_OCD_TH_750mA,               /* Overcurrent threshold (OCD_TH register). */
+    L6474_CONFIG_OC_SD_ENABLE,        /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
+    L6474_CONFIG_EN_TQREG_TVAL_USED,  /* Torque regulation method (EN_TQREG field of CONFIG register). */
+    L6474_STEP_SEL_1,               /* Step selection (STEP_SEL field of STEP_MODE register). */
+    L6474_SYNC_SEL_1,               /* Sync selection (SYNC_SEL field of STEP_MODE register). */
+    L6474_FAST_STEP_12us,             /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
+    L6474_TOFF_FAST_8us,              /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
+    3,                                /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
+    21,                               /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
+    L6474_CONFIG_TOFF_044us,          /* Target Swicthing Period (field TOFF of CONFIG register). */
+    L6474_CONFIG_SR_320V_us,          /* Slew rate (POW_SR field of CONFIG register). */
+    L6474_CONFIG_INT_16MHZ,           /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
+    L6474_ALARM_EN_OVERCURRENT |
+    L6474_ALARM_EN_THERMAL_SHUTDOWN |
+    L6474_ALARM_EN_THERMAL_WARNING |
+    L6474_ALARM_EN_UNDERVOLTAGE |
+    L6474_ALARM_EN_SW_TURN_ON |
+    L6474_ALARM_EN_WRONG_NPERF_CMD    /* Alarm (ALARM_EN register). */
+};
+
+Serial pc(USBTX, USBRX);
+
+AnalogIn eixo_X(A0);
+AnalogIn eixo_Y(A1);
+
+
+
+float x;
+float y;
+float w;
+float u;
+
+unsigned int minspeed = 1600;
+
+int step = 0x08;
+
+int speed1;
+int speed2;
+
 
 /* Motor Control Component. */
 L6474 *motor1;
@@ -121,184 +168,55 @@
 
     
     /*----- Moving. -----*/
-    
-    /* Printing to the console. */
-    printf("--> Moving backward: M1 %d steps, M2 %d steps.\r\n", STEPS >> 1, STEPS);
-
-    
-    /* Moving N steps in the backward direction. */
-    motor1->move(StepperMotor::BWD, STEPS >> 1);
-    motor2->move(StepperMotor::BWD, STEPS);
-    
-    /* Waiting while the motor is active. */
-    motor1->wait_while_active();
-    motor2->wait_while_active();
-
-    /* Getting current position. */
-    position1 = motor1->get_position();
-    position2 = motor2->get_position();
-    
-    /* Printing to the console. */
-    printf("    Position: M1 %d, M2 %d.\r\n", position1, position2);
-    printf("--> Setting Home.\r\n");
-
-    /* Setting the current position to be the home position. */
-    motor1->set_home();
-    motor2->set_home();
-
-    /* Waiting 2 seconds. */
-    wait_ms(DELAY_1);
-
-
-    /*----- Going to a specified position. -----*/
-
-    /* Printing to the console. */
-    printf("--> Going to position: M1 %d, M2 %d.\r\n", STEPS, STEPS >> 1);
-    
-    /* Requesting to go to a specified position. */
-    motor1->go_to(STEPS);
-    motor2->go_to(STEPS >> 1);
-    
-    /* Waiting while the motor is active. */
-    motor1->wait_while_active();
-    motor2->wait_while_active();
-
-    /* Getting current position. */
-    position1 = motor1->get_position();
-    position2 = motor2->get_position();
-    
-    /* Printing to the console. */
-    printf("    Position: M1 %d, M2 %d.\r\n", position1, position2);
-    
-    /* Waiting 2 seconds. */
-    wait_ms(DELAY_1);
-
-    
-    /*----- Going Home. -----*/
-
-    /* Printing to the console. */
-    printf("--> Going Home.\r\n");
-    
-    /* Requesting to go to home. */
-    motor1->go_home();
-    motor2->go_home();
-    
-    /* Waiting while the motor is active. */
-    motor1->wait_while_active();
-    motor2->wait_while_active();
-
-    /* Getting current position. */
-    position1 = motor1->get_position();
-    position2 = motor2->get_position();
-
-    /* Printing to the console. */
-    printf("    Position: M1 %d, M2 %d.\r\n", position1, position2);
-
-    /* Waiting 2 seconds. */
-    wait_ms(DELAY_1);
 
 
-    /*----- Running. -----*/
-
-    /* Printing to the console. */
-    printf("--> M1 running backward, M2 running forward.\r\n");
-
-    /* Requesting to run backward. */
-    motor1->run(StepperMotor::BWD);
-    motor2->run(StepperMotor::FWD);
-
-    /* Waiting until delay has expired. */
-    wait_ms(DELAY_2);
-
-    /* Getting current speed. */
-    int speed1 = motor1->get_speed();
-    int speed2 = motor2->get_speed();
-
-    /* Printing to the console. */
-    printf("    Speed: M1 %d, M2 %d.\r\n", speed1, speed2);
-
-    /*----- Increasing the speed while running. -----*/
-
-    /* Printing to the console. */
-    printf("--> Increasing the speed while running.\r\n");
-
-    /* Increasing the speed. */
-    motor1->set_max_speed(SPEED_1);
-    motor2->set_max_speed(SPEED_1);
-
-    /* Waiting until delay has expired. */
-    wait_ms(DELAY_2);
-
-    /* Getting current speed. */
-    speed1 = motor1->get_speed();
-    speed2 = motor2->get_speed();
-
-    /* Printing to the console. */
-    printf("    Speed: M1 %d, M2 %d.\r\n", speed1, speed2);
-
-
-    /*----- Decreasing the speed while running. -----*/
-
-    /* Printing to the console. */
-    printf("--> Decreasing the speed while running.\r\n");
-
-    /* Decreasing the speed. */
-    motor1->set_max_speed(SPEED_2);
-    motor2->set_max_speed(SPEED_2);
-
-    /* Waiting until delay has expired. */
-    wait_ms(DELAY_3);
-
-    /* Getting current speed. */
-    speed1 = motor1->get_speed();
-    speed2 = motor2->get_speed();
-
-    /* Printing to the console. */
-    printf("    Speed: M1 %d, M2 %d.\r\n", speed1, speed2);
-
-
-    /*----- Requiring hard-stop while running. -----*/
-
-    /* Printing to the console. */
-    printf("--> Requiring hard-stop while running.\r\n");
-
-    /* Requesting to immediatly stop. */
-    motor1->hard_stop();
-    motor2->hard_stop();
-
-    /* Waiting while the motor is active. */
-    motor1->wait_while_active();
-    motor2->wait_while_active();
-
-    /* Waiting 2 seconds. */
-    wait_ms(DELAY_1);
-
-
-    /*----- Infinite Loop. -----*/
-
-    /* Printing to the console. */
-    printf("--> Infinite Loop...\r\n");
-
-    /* Setting the current position to be the home position. */
-    motor1->set_home();
-    motor2->set_home();
-
-    /* Infinite Loop. */
     while(true) {
-        /* Requesting to go to a specified position. */
-        motor1->go_to(STEPS >> 1);
-        motor2->go_to(- (STEPS >> 1));
-
-        /* Waiting while the motor is active. */
-        motor1->wait_while_active();
-        motor2->wait_while_active();
-
-        /* Requesting to go to a specified position. */
-        motor1->go_to(- (STEPS >> 1));
-        motor2->go_to(STEPS >> 1);
-
-        /* Waiting while the motor is active. */
-        motor1->wait_while_active();
-        motor2->wait_while_active();
+        u = eixo_X.read();
+        w = eixo_Y.read();
+        
+  //      wait (1.5);
+  //      pc.printf ("X:   %f \r\n",u);
+  //      pc.printf ("Y:    %f  \r\n",w);
+  
+        if (u>0.520) {
+           motor1->set_min_speed(minspeed);
+           motor1->set_step_mode(step_mode_t_1);
+           motor1->run(StepperMotor::FWD);
+           speed1 = motor1->get_speed();
+           printf("velocidade: motor 1: %d \r\n",speed1);
+           printf("movendo motor 1 pra frente\r\n");
+           }
+            else{ if(u<0.455) {
+                motor1->set_min_speed(minspeed);
+                motor1->run(StepperMotor::BWD);
+                speed1 = motor1->get_speed();
+                printf("velocidade: motor 1: %d \r\n",speed1);
+                printf("movendo motor 1 pra tras\r\n");
+                }
+               else 
+               motor1->hard_stop();
+               printf("parado1\r\n");
+               printf("velocidade: motor 1: %d \r\n",speed1);
+               }
+        
+       if (w>0.535) {
+           motor2->set_min_speed(minspeed);
+           motor2->run(StepperMotor::FWD);
+           speed2 = motor2->get_speed();
+           printf("velocidade: motor 1: %d \r\n",speed2);
+           printf("movendo motor 2 pra frente\r\n");
+           }
+            else{ if(w<0.491) {
+                motor1->set_min_speed(minspeed);
+                motor2->run(StepperMotor::BWD);
+                speed2 = motor1->get_speed();
+                printf("velocidade: motor 1: %d \r\n",speed2);
+                printf("movendo motor 2 pra tras\r\n");
+               }
+                else
+                motor2->hard_stop();
+                 printf("parado2\r\n");
+                 printf("velocidade: motor 2: %d \r\n",speed2);
+               }
     }
-}
+}
\ No newline at end of file