Diff: main.cpp

Revision:

24:37f139e067b2

Parent:

23:3110010d98a0

Child:

25:281c8e913db4

--- a/main.cpp	Sat Apr 06 14:51:15 2019 +0000
+++ b/main.cpp	Wed Jun 12 15:53:00 2019 +0000
@@ -1,152 +1,209 @@
 #include "mbed.h"
-#include "L6208.h"
+#include "L6206.h"
+
+#define MAX_MOTOR (2)
 
-#define VREFA_PWM_PIN D3
-#define VREFB_PWM_PIN D9
+static volatile uint16_t gLastError;
+static volatile uint8_t gStep = 0;
 
-l6208_init_t init =
+int current_pose = 0;
+int speed = 0;
+
+L6206_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)
+    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
+//Utility
 InterruptIn button(USER_BUTTON);
-DigitalOut led(LED1);
-
-// Motor Control
-L6208 *motor;
+DigitalOut led(LED1); //Change?
 
-InterruptIn end0(PC_10, PullUp);
-InterruptIn end1(PC_11, PullUp);
-InterruptIn enc(PC_12, PullUp);
-
-int current_pose = 0;
-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(StepperMotor::FWD);
+  motor->run(0, BDCMotor::FWD);
+  motor->run(1, BDCMotor::FWD);
 }
 
-void button_int_handler()
+void button_int_handler(unsigned int motorId)
 {
-    printf("POSITION: %d\n\r", motor->get_position());
+    printf("MOTOR SPEED: %d\n\r", motor->get_speed(motorId)); 
     motor_zero();
 }
 
-void end0_int_handler()
+void my_error_handler(uint16_t error)
+{
+  /* Backup error number */
+  gLastError = error;
+  
+  /* Enter your own code here */
+}
+
+void my_flag_irq_handler(void)
 {
-  printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
+  /* 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));
 }
 
 void end1_int_handler()
 {
-  motor->hard_stop();
+  motor->hard_stop(0);
+  motor->hard_stop(1); //or hard_hiz(); for disabling the bridge?
 
-  motor->run(StepperMotor::BWD);
+   motor->run(0, BDCMotor::BWD);
+   motor->run(1, BDCMotor::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
+// CAN, to revise
 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==0x010)
+    if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==6))) //Primo motore
     {
-      pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
-      printf("CAN: mess %d\n\r", pose);
+      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, "\% ");
       
-      
-      if (pose == 1) 
+      //CAN MESSAGE WITH SPEED TO REVISE
+      //Ci sarebbe anche il set speed.
+      if (speedMap == 0) 
       {
-        motor->run(StepperMotor::FWD);
+        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); 
       }
-      else if (pose == 0)
+      else if (speedMap>0)
+        {
+            motor->run(0,BDCMotor::FWD);
+            motor->set_speed(0,(unsigned int) speedMap);
+        }
+      else if (speedMap<0)
       {
-        motor->soft_stop();
-        current_pose= motor->get_position();
-        motor->go_to(current_pose);
-      }
-      else if (pose == 2)
-      {
-        motor->run(StepperMotor::BWD);
+        motor->run(0,BDCMotor::BWD);
+        motor->set_speed(0,(unsigned int) -speedMap);
       }
       else 
       {
-        motor->soft_stop();
-        current_pose= motor->get_position();
-        motor->go_to(current_pose);
+        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, "\% ");
       
-     // motor->go_to(pose);
-    }
-  }
+      //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); 
+      }
+      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
+      }
+     }
 }
-
+//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 
-  motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
   
-  if (motor->init(&init) != COMPONENT_OK)
+#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) 
   {
     printf("ERROR: vvMotor Init\n\r");
     exit(EXIT_FAILURE);
   }
 
-  motor->attach_error_handler(&motor_error_handler);
-  
-  end0.rise(&end0_int_handler);
-  end1.rise(&end1_int_handler);
-  end1.fall(&motor_set_home);
+  motor->attach_flag_interrupt(my_flag_irq_handler);
+  motor->attach_error_handler(my_error_handler);
+
+ 
   
   button.rise(&button_int_handler);
   
-  motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
   printf("DONE: Motor Init\n\r");
   
   // CAN Initialization