Diff: main.cpp

Revision:

26:c18d6aaa474a

Parent:

25:84d2bd93bf97

Child:

27:bf0109fb61c3

--- a/main.cpp	Wed Jun 12 16:03:30 2019 +0000
+++ b/main.cpp	Wed Jun 19 16:29:11 2019 +0000
@@ -1,7 +1,10 @@
 #include "mbed.h"
 #include "L6206.h"
 
-#define MAX_MOTOR (2)
+#define JOINT_SET_SPEED 20
+
+#define JOINT_ID 1
+
 
 static volatile uint16_t gLastError;
 static volatile uint8_t gStep = 0;
@@ -20,7 +23,7 @@
 };
 
 L6206 *motor;
-InterruptIn my_button_irq(USER_BUTTON); /* User button on Nucleo board */
+
 Thread canrxa;
 
 //Utility
@@ -82,108 +85,41 @@
     }
   }  
 }
-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(0);
-  motor->hard_stop(1); //or hard_hiz(); for disabling the bridge?
-
-   motor->run(0, BDCMotor::BWD);
-   motor->run(1, BDCMotor::BWD);
-
-  printf("END1: Pressed\n\r");
-}
-
-
 
 // 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>>8 == 20) && (messageIn.id & 0x00FF==6))) //Primo 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 %f\n\r", speedMap);
-      
-      //CAN MESSAGE WITH SPEED TO REVISE
-      //Ci sarebbe anche il set speed.
-      if (speedMap == 0) 
-      {
-        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 (speedMap>0)
-        {
-            motor->run(0,BDCMotor::FWD);
-            motor->set_speed(0,(unsigned int) speedMap);
-        }
-      else if (speedMap<0)
-      {
-        motor->run(0,BDCMotor::BWD);
-        motor->set_speed(0,(unsigned int) -speedMap);
-      }
-      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
+    if(can1.read(messageIn))
     {
-      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 %f\n\r", speedMap);
-      
-      //CAN MESSAGE WITH SPEED TO REVISE
-      //Ci sarebbe anche il set speed.
-      if (speedMap == 0) 
+      if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
       {
-        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); 
+          speed = 0;
+          speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
+          
+          motor->set_speed(0, speed);
+          (speed > 0) ? motor->run(0, BDCMotor::BWD) : motor->run(0, BDCMotor::FWD);
+          
+          printf("CAN: mess %d\n\r", speed);
       }
-      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
-      }
-     }
-     }
+    }
+    
+    wait(0.1);
+  }
 }
-//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);
+  can1.frequency(500000);
   messageIn.format=CANExtended;
-  messageOut.format=CANExtended;
+  
   // Motor Initialization 
   
 #ifdef TARGET_STM32F429
@@ -216,6 +152,6 @@
   
   while(true)
   {
-    wait(1000);
+    wait(1);
   }
 }