Revision 25:281c8e913db4, committed 2019-06-12

Comitter:

stebonicelli

Date:

Wed Jun 12 16:10:21 2019 +0000

Parent:

24:37f139e067b2

Child:

26:44175c51a820

Commit message:

reverted

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/X-NUCLEO-IHM05A1.lib	Wed Jun 12 16:10:21 2019 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/X-NUCLEO-IHM05A1/#c73faac7197f

--- a/X_NUCLEO_IHM04A1.lib	Wed Jun 12 15:53:00 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/teams/ST/code/X_NUCLEO_IHM04A1/#d16ad1d58ea1

--- a/main.cpp	Wed Jun 12 15:53:00 2019 +0000
+++ b/main.cpp	Wed Jun 12 16:10:21 2019 +0000
@@ -1,209 +1,152 @@
 #include "mbed.h"
-#include "L6206.h"
+#include "L6208.h"
+
+#define VREFA_PWM_PIN D3
+#define VREFB_PWM_PIN D9
 
-#define MAX_MOTOR (2)
+l6208_init_t init =
+{
+  8000,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
+  80,              //Acceleration current torque in % (from 0 to 100)
+  8000,            //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
+  80,              //Deceleration current torque in % (from 0 to 100)
+  8000,            //Running speed in step/s or (1/16)th step/s for microstep modes
+  80,              //Running current torque in % (from 0 to 100)
+  40,              //Holding current torque in % (from 0 to 100)
+  STEP_MODE_1_16,  //Step mode via enum motorStepMode_t
+  FAST_DECAY,      //Decay mode via enum motorDecayMode_t
+  0,               //Dwelling time in ms
+  FALSE,           //Automatic HIZ STOP
+  100000           //VREFA and VREFB PWM frequency (Hz)
+};
 
-static volatile uint16_t gLastError;
-static volatile uint8_t gStep = 0;
+Thread canrxa;
+
+// Utility
+InterruptIn button(USER_BUTTON);
+DigitalOut led(LED1);
+
+// Motor Control
+L6208 *motor;
+
+InterruptIn end0(PC_10, PullUp);
+InterruptIn end1(PC_11, PullUp);
+InterruptIn enc(PC_12, PullUp);
 
 int current_pose = 0;
-int speed = 0;
-
-L6206_init_t init =
-{
-    L6206_CONF_PARAM_PARALLE_BRIDGES,
-    {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
-    {100,100,100,100},
-    {FORWARD,FORWARD,BACKWARD,FORWARD},
-    {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
-    {FALSE,FALSE}
-};
-
-L6206 *motor;
-InterruptIn my_button_irq(USER_BUTTON); /* User button on Nucleo board */
-Thread canrxa;
-
-//Utility
-InterruptIn button(USER_BUTTON);
-DigitalOut led(LED1); //Change?
-
+int pose = 0;
 
 void motor_error_handler(uint16_t error)
 {
   printf("ERROR: Motor Runtime\n\r");
-  while(1){}; 
+  while(1){} 
 }
 
 void motor_zero()
 {
-  motor->run(0, BDCMotor::FWD);
-  motor->run(1, BDCMotor::FWD);
+  motor->run(StepperMotor::FWD);
 }
 
-void button_int_handler(unsigned int motorId)
+void button_int_handler()
 {
-    printf("MOTOR SPEED: %d\n\r", motor->get_speed(motorId)); 
+    printf("POSITION: %d\n\r", motor->get_position());
     motor_zero();
 }
 
-void my_error_handler(uint16_t error)
-{
-  /* Backup error number */
-  gLastError = error;
-  
-  /* Enter your own code here */
-}
-
-void my_flag_irq_handler(void)
+void end0_int_handler()
 {
-  /* Code to be customised */
-  /************************/
-  /* Get the state of bridge A */
-  uint16_t bridgeState  = motor->get_bridge_status(0);
-  
-  if (bridgeState == 0) {
-    if ((motor->get_device_state(0) != INACTIVE)||
-        (motor->get_device_state(1) != INACTIVE)) {
-      /* Bridge A was disabling due to overcurrent or over temperature */
-      /* When at least on of its  motor was running */
-        my_error_handler(0XBAD0);
-    }
-  }
-  
-  /* Get the state of bridge B */
-  bridgeState  = motor->get_bridge_status(1);
-  
-  if (bridgeState == 0)  {
-    if ((motor->get_device_state(2) != INACTIVE)||
-        (motor->get_device_state(3) != INACTIVE)) {
-      /* Bridge A was disabling due to overcurrent or over temperature */
-      /* When at least on of its  motor was running */
-        my_error_handler(0XBAD1);
-    }
-  }  
-}
-void end0_int_handler(unsigned int motorId)
-{
-  printf("END0: Pressed\n\rSPEED: %d\n\r", motor->get_speed(motorId));
+  printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
 }
 
 void end1_int_handler()
 {
-  motor->hard_stop(0);
-  motor->hard_stop(1); //or hard_hiz(); for disabling the bridge?
+  motor->hard_stop();
 
-   motor->run(0, BDCMotor::BWD);
-   motor->run(1, BDCMotor::BWD);
+  motor->run(StepperMotor::BWD);
 
   printf("END1: Pressed\n\r");
 }
 
-
+void motor_set_home()
+{
+  motor->hard_stop();
+  motor->set_home();
+  motor->go_to(0);
+  
+  current_pose = 0;
+  pose = 0;
+}
 
-// CAN, to revise
+// CAN
 CAN can1(PB_12, PB_13);     // RX, TX
 
 CANMessage messageIn;
 CANMessage messageOut;
 
-
 int filter = can1.filter(0x010, 0x4FF, CANStandard);
 
 void canrx()
 {
   while(1)
   {    
-    if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==6))) //Primo motore
+    if(can1.read(messageIn,filter)&&messageIn.id==0x010)
     {
-      speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno
-      float speedMap=(speed-127)/127*100;
-      printf("CAN: mess %d\n\r", speedMap, "\% ");
+      pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
+      printf("CAN: mess %d\n\r", pose);
       
-      //CAN MESSAGE WITH SPEED TO REVISE
-      //Ci sarebbe anche il set speed.
-      if (speedMap == 0) 
+      
+      if (pose == 1) 
       {
-        motor->set_speed(0,0); //There's no soft stop. could it work like this?
-        //current_speed= motor->get_speed(0); We could do lie this?
-        //motor->go_to(current_pose); 
+        motor->run(StepperMotor::FWD);
       }
-      else if (speedMap>0)
-        {
-            motor->run(0,BDCMotor::FWD);
-            motor->set_speed(0,(unsigned int) speedMap);
-        }
-      else if (speedMap<0)
+      else if (pose == 0)
       {
-        motor->run(0,BDCMotor::BWD);
-        motor->set_speed(0,(unsigned int) -speedMap);
+        motor->soft_stop();
+        current_pose= motor->get_position();
+        motor->go_to(current_pose);
+      }
+      else if (pose == 2)
+      {
+        motor->run(StepperMotor::BWD);
       }
       else 
       {
-        motor->set_speed(0,0); //Riportare errore? NO
-      }
-     }
-     else if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==7))) //Secondo motore
-    {
-      speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno
-      float speedMap=(speed-127)/127*100;
-      printf("CAN: mess %d\n\r", speedMap, "\% ");
-      
-      //CAN MESSAGE WITH SPEED TO REVISE
-      //Ci sarebbe anche il set speed.
-      if (speedMap == 0) 
-      {
-        motor->set_speed(1,0); //There's no soft stop. could it work like this?
-        //current_speed= motor->get_speed(0); We could do lie this?
-        //motor->go_to(current_pose); 
+        motor->soft_stop();
+        current_pose= motor->get_position();
+        motor->go_to(current_pose);
       }
-      else if (speedMap>0)
-        {
-            motor->run(1,BDCMotor::FWD);
-            motor->set_speed(1,(unsigned int) speedMap);
-        }
-      else if (speedMap<0)
-      {
-        motor->run(1,BDCMotor::BWD);
-        motor->set_speed(1,(unsigned int) -speedMap);
-      }
-      else 
-      {
-        motor->set_speed(0,0); //Riportare errore? NO
-      }
-     }
+      
+      
+     // motor->go_to(pose);
+    }
+  }
 }
-//Aggiungere un get speed,FATTO  ci sarebbe anche il set_speed 
-        //Dentro la libreria controllare se c'è controllo di Duty cycle FATTO
-        //motor->go_to(current_pose);
+
 
 /* Main ----------------------------------------------------------------------*/
 
 int main()
 {
   can1.frequency(125000);
-  messageIn.format=CANExtended;
-  messageOut.format=CANExtended;
-  // Motor Initialization 
   
-#ifdef TARGET_STM32F429
-    motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3);
-#else
-    motor = new L6206(D2, A4, D5, D4, A0, A1);
-#endif
- 
-  if (motor->init(&init) != COMPONENT_OK) 
+  // Motor Initialization 
+  motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
+  
+  if (motor->init(&init) != COMPONENT_OK)
   {
     printf("ERROR: vvMotor Init\n\r");
     exit(EXIT_FAILURE);
   }
 
-  motor->attach_flag_interrupt(my_flag_irq_handler);
-  motor->attach_error_handler(my_error_handler);
-
- 
+  motor->attach_error_handler(&motor_error_handler);
+  
+  end0.rise(&end0_int_handler);
+  end1.rise(&end1_int_handler);
+  end1.fall(&motor_set_home);
   
   button.rise(&button_int_handler);
   
+  motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
   printf("DONE: Motor Init\n\r");
   
   // CAN Initialization