Revision 26:44175c51a820, committed 2019-06-18

Comitter:

stebonicelli

Date:

Tue Jun 18 16:28:38 2019 +0000

Parent:

25:281c8e913db4

Child:

27:275ba9c137c9

Commit message:

Can protocol implemented

Changed in this revision

--- a/main.cpp	Wed Jun 12 16:10:21 2019 +0000
+++ b/main.cpp	Tue Jun 18 16:28:38 2019 +0000
@@ -4,6 +4,10 @@
 #define VREFA_PWM_PIN D3
 #define VREFB_PWM_PIN D9
 
+#define JOINT_SET_SPEED 20
+
+#define JOINT_ID 1
+
 l6208_init_t init =
 {
   8000,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
@@ -33,18 +37,17 @@
 InterruptIn end1(PC_11, PullUp);
 InterruptIn enc(PC_12, PullUp);
 
-int current_pose = 0;
-int pose = 0;
+int32_t speed = 0;
 
 void motor_error_handler(uint16_t error)
 {
-  printf("ERROR: Motor Runtime\n\r");
-  while(1){} 
+    printf("ERROR: Motor Runtime\n\r");
+    while(1){} 
 }
 
 void motor_zero()
 {
-  motor->run(StepperMotor::FWD);
+    motor->run(StepperMotor::FWD);
 }
 
 void button_int_handler()
@@ -55,16 +58,15 @@
 
 void end0_int_handler()
 {
-  printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
+    printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
 }
 
 void end1_int_handler()
 {
-  motor->hard_stop();
+    motor->hard_stop();
+    motor->run(StepperMotor::BWD);
 
-  motor->run(StepperMotor::BWD);
-
-  printf("END1: Pressed\n\r");
+    printf("END1: Pressed\n\r");
 }
 
 void motor_set_home()
@@ -72,9 +74,6 @@
   motor->hard_stop();
   motor->set_home();
   motor->go_to(0);
-  
-  current_pose = 0;
-  pose = 0;
 }
 
 // CAN
@@ -83,42 +82,25 @@
 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))
     {
-      pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
-      printf("CAN: mess %d\n\r", pose);
-      
-      
-      if (pose == 1) 
-      {
-        motor->run(StepperMotor::FWD);
-      }
-      else if (pose == 0)
+      if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
       {
-        motor->soft_stop();
-        current_pose= motor->get_position();
-        motor->go_to(current_pose);
-      }
-      else if (pose == 2)
-      {
-        motor->run(StepperMotor::BWD);
+          speed = 0;
+          speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
+          
+          motor->set_max_speed(speed);
+          (speed > 0) ? motor->run(StepperMotor::BWD) : motor->run(StepperMotor::FWD);
+          
+          printf("CAN: mess %d\n\r", speed);
       }
-      else 
-      {
-        motor->soft_stop();
-        current_pose= motor->get_position();
-        motor->go_to(current_pose);
-      }
-      
-      
-     // motor->go_to(pose);
     }
+    
+    wait(0.1);
   }
 }
 
@@ -127,7 +109,7 @@
 
 int main()
 {
-  can1.frequency(125000);
+  can1.frequency(500000);
   
   // Motor Initialization 
   motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
@@ -155,12 +137,10 @@
   
   printf("DONE: CAN Init\n\r");
   
-
-  
   printf("Running!\n\r");
   
   while(true)
   {
-    wait(1000);
+    wait(1);
   }
 }