Revision 0:2c5d9f3acfb8, committed 2019-10-03

Comitter:

edoardoVacchetto

Date:

Thu Oct 03 17:29:28 2019 +0000

Child:

1:6cca05643958

Commit message:

arm base stepper double serial

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/X-NUCLEO-IHM05A1.lib	Thu Oct 03 17:29:28 2019 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/X-NUCLEO-IHM05A1/#c73faac7197f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 03 17:29:28 2019 +0000
@@ -0,0 +1,195 @@
+#include "mbed.h"
+#include "L6208.h"
+
+#define Rx_Buff_Dim 12
+
+#define VREFA_PWM_PIN D3
+#define VREFB_PWM_PIN D9
+#define BAUDRATE 9600
+#define JOINT 2
+
+l6208_init_t init =
+{
+  8100,            //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)
+  8100,            //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)
+};
+
+// Motor Control
+L6208 *motor;
+DigitalIn end0(PA_5);   // endstop
+Serial s_rx(PC_10, PC_11); // comunication usart
+Serial Pc_Stat(PA_2, PA_3);// status usart
+Timer timeOuter;
+
+char dataRxBuffer[Rx_Buff_Dim];  
+volatile int rxBufferPtr;
+
+float pose, current_pose;
+int8_t speed, current_speed;
+
+void motor_error_handler(uint16_t error);   
+void zero();
+void fmotor();
+void runCommand();
+void serialrx();
+
+bool posMode = false;
+
+int main(){
+    s_rx.baud(BAUDRATE);
+    Pc_Stat.baud(115200);
+    
+    // Motor Initialization
+    motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
+    motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
+    if (motor->init(&init) != COMPONENT_OK)
+    {
+      Pc_Stat.printf("ERROR: vvMotor Init\n\r");
+      exit(EXIT_FAILURE);
+    }
+    
+    motor->attach_error_handler(&motor_error_handler);
+        
+    printf("DONE: Motor Init\n\r");
+    
+    Pc_Stat.printf("Running!\n\r");
+        
+    motor->set_home();
+
+    s_rx.printf("19 1\n");
+
+    timeOuter.start();
+    
+    while(true)
+    {
+      serialrx();
+      if (timeOuter.read_ms() > 2000) {
+          current_speed = 0;
+          timeOuter.reset();
+          Pc_Stat.printf("Command timeout!!\n");
+      }
+      //wait (0.001);
+      if (!posMode) fmotor();
+      
+    }
+}
+  
+void motor_error_handler(uint16_t error){
+  Pc_Stat.printf("ERROR: Motor Runtime\n\r");
+}  
+  
+void zero(){
+  Pc_Stat.printf("Zeroing (%d)... ", (int) end0);
+  //motor->run(StepperMotor::BWD);
+  
+  int osc_pos = 10000;
+    
+  if (!end0)  {
+      motor->hard_stop();
+      motor->set_home();
+
+      motor->go_to(-8000);
+      while(motor->get_position() != -8000);
+  }
+
+  motor->set_home();
+  
+  for (int i = 0; i < 4; i++) {
+      motor->go_to(osc_pos);
+      while(end0 && motor->get_position() != osc_pos);
+      if (!end0) {
+          motor->hard_stop();
+          motor->set_home();
+          osc_pos = 5000 * (osc_pos > 0 ? 1 : -1);
+          motor->go_to(osc_pos);
+          while(motor->get_position() != osc_pos);
+          motor->set_home();
+          Pc_Stat.printf("OK!\n");
+          return;
+      } else { 
+          osc_pos *= -2;
+      }
+  }
+
+  Pc_Stat.printf("ZERO FAILURE!\n");
+}  
+
+void fmotor()
+{ 
+    unsigned int spd = abs(current_speed*80);
+    
+    if (spd) {
+        motor->set_max_speed(spd);
+        motor->run(current_speed >= 0 ? 
+            StepperMotor::FWD : StepperMotor::BWD);
+    } else {
+        motor->hard_stop();
+        current_pose = motor->get_position();
+        motor->go_to(current_pose);
+    }
+    wait(0.001);
+}
+
+void runCommand() {
+    int joint, spd;
+    
+    current_speed = 0;
+    timeOuter.reset();
+          
+    sscanf(dataRxBuffer, "%d %d", &joint, &spd);
+    //Pc_Stat.printf("joint: %d, value: %d", joint, spd);
+    posMode = false;
+    
+    switch(joint){
+        case 10:
+            zero();
+            break;
+        case 11:
+            posMode = true;
+            Pc_Stat.printf("Moving(position)... ");
+            motor->set_max_speed(8000);
+            motor->go_to(spd);
+            Pc_Stat.printf("Done!\n");
+            break;
+        case 12:
+            Pc_Stat.printf("Moving(velocity)... ");
+            current_speed = spd;
+            s_rx.printf("15 %d\n",motor->get_position());
+            break;
+        case 13:
+            motor->hard_stop();
+            Pc_Stat.printf("STOP.\n");
+            break;           
+        default:
+            Pc_Stat.printf("Unknown index %d - data: %d", joint, spd);
+            break;
+    }
+}
+
+void serialrx()
+{  
+    while(s_rx.readable()) 
+    {
+      char c = s_rx.getc();
+      Pc_Stat.printf("%c",c);
+      
+      if (c == '\n' || rxBufferPtr >= Rx_Buff_Dim - 1) {
+          dataRxBuffer[rxBufferPtr] = '\0';
+          runCommand();
+          rxBufferPtr = 0;
+          
+      } else {
+          dataRxBuffer[rxBufferPtr++] = c;
+      }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Oct 03 17:29:28 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file