Diff: main.cpp

Revision:

31:5d6a97adae07

Parent:

30:c40b060795a2

Child:

32:465e41868fe4

--- a/main.cpp	Wed Sep 04 16:31:22 2019 +0000
+++ b/main.cpp	Thu Sep 05 20:16:02 2019 +0000
@@ -1,9 +1,9 @@
 #include "mbed.h"
 #include "L6208.h"
-
+ 
 #define VREFA_PWM_PIN D3
 #define VREFB_PWM_PIN D9
-
+#define SEND_FREQUENCY 10 //Hz
 l6208_init_t init =
 {
   8000,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
@@ -19,21 +19,21 @@
   FALSE,           //Automatic HIZ STOP
   100000           //VREFA and VREFB PWM frequency (Hz)
 };
-
+ 
 Thread cantxa(osPriorityNormal);
 Thread canrxa(osPriorityNormal);
-
+ 
 // Utility
 //InterruptIn button(USER_BUTTON);
 DigitalOut led(LED1);
-
+ 
 // Motor Control
 L6208 *motor;
-
+ 
 InterruptIn end1(USER_BUTTON, PullUp);
 DigitalIn end0(PA_5, PullUp);
 InterruptIn enc(PC_12, PullUp);
-
+ 
 typedef enum
 {
   JOINT_SET_SPEED = 20,
@@ -46,7 +46,7 @@
   JOINT_MAXTORQUE,
   JOINT_ZERO,
 }CAN_COMMANDS;
-
+ 
 typedef enum
 {
   BASE=1,
@@ -59,7 +59,7 @@
   CAMERA1,
   CAMERA2,
 }JOINT;
-
+ 
 float pose, current_pose;
 float speed, current_speed;
 void zero()
@@ -82,38 +82,40 @@
   id |= 0x80000000;              // Send in Extended Frame Format.
   return id;
 }
-
+ 
+double to_rad(double angle)
+{
+  return angle*0.0174533;
+}
 double angle_deparse (long int pose, float offset)
 {
   offset = offset * 0.00872664625;
-  double angle = pose *0.00000487012987; //do something 0,0004791666667
-  angle = (angle - offset) * 100;
+  double angle = pose *0.000487012987; //do something 0,0004791666667
+  angle = (angle - offset);
   return angle;
 }
-void motor_error_handler(uint16_t error)
+ void motor_error_handler(uint16_t error)
 {
   printf("ERROR: Motor Runtime\n\r");
 
 }
-
-
-
+ 
 void end1_int_handler()
 {
  // motor->hard_stop();
-
+ 
   motor->run(StepperMotor::FWD);
-
+ 
   printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
 }
-
-
+ 
+ 
 // CAN
 CAN can1(PB_8, PB_9);     // RX, TX
-
+ 
 CANMessage messageIn;
 CANMessage messageOut;
-
+ 
 void cantx ()
 {
     while(1)
@@ -128,23 +130,23 @@
     messageOut.data[2]=_pose >>8;
     messageOut.data[1]=_pose >>16;
     messageOut.data[0]=_pose >>24;
-
+ 
     int status = can1.write(messageOut);
     led=!status;
     printf("CAN send CURRENT POSITION Joint status %d : pose %f\t\n",status, pose);
     }
     
-
-
-
+ 
+ 
+ 
 }
 void cantx_ISR()
 {
     cantx();
-    osDelay(60);
+    osDelay(1/SEND_FREQUENCY*1000);
 }
-
-
+ 
+ 
 void canrx()
 {
   while(1)
@@ -155,8 +157,8 @@
       speed=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
       printf("CAN: mess %d\n\r", speed);
       current_speed=speed-100;
-
-
+ 
+ 
       if (current_speed>0)
       {
         motor->set_max_speed(current_speed*80);
@@ -167,7 +169,7 @@
         motor->set_max_speed(current_speed*80);
         motor->run(StepperMotor::BWD);
       }
-
+ 
       else
       {
         motor->soft_stop();
@@ -180,9 +182,8 @@
     {
       if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
       {
-        motor->hard_stop();
-        motor->set_max_speed(5000);
-        motor->run(StepperMotor::BWD);
+        zero();
+        motor->wait_while_active();
       }
     }
     
@@ -193,9 +194,9 @@
     canrx();
     osDelay(10);
 }
-
+ 
 /* Main ----------------------------------------------------------------------*/
-
+ 
 int main()
 {
   
@@ -209,28 +210,30 @@
     printf("ERROR: vvMotor Init\n\r");
     exit(EXIT_FAILURE);
   }
-
+ 
   motor->attach_error_handler(&motor_error_handler);
-
-
+ 
+ 
   end1.rise(&end1_int_handler);
-
+ 
   printf("DONE: Motor Init\n\r");
-
+ 
   // CAN Initialization
-    zero();
+    
     
   canrxa.start(canrx_ISR);
   cantxa.start(cantx_ISR);
   
   printf("DONE: CAN Init\n\r");
-
-
-
+ 
+ 
+ 
   printf("Running!\n\r");
-
+  
+  zero();
+  
   while(true)
   {
     wait(1000);
   }
-}
+}
\ No newline at end of file