Diff: main.cpp

Revision:

27:bf0109fb61c3

Parent:

26:c18d6aaa474a

Child:

28:637b6f726971

--- a/main.cpp	Wed Jun 19 16:29:11 2019 +0000
+++ b/main.cpp	Fri Jun 21 07:43:15 2019 +0000
@@ -3,7 +3,7 @@
 
 #define JOINT_SET_SPEED 20
 
-#define JOINT_ID 1
+#define JOINT_ID 3
 
 
 static volatile uint16_t gLastError;
@@ -14,10 +14,10 @@
 
 L6206_init_t init =
 {
-    L6206_CONF_PARAM_PARALLE_BRIDGES,
+    PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B,
     {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},
+    {FORWARD,BACKWARD,FORWARD,BACKWARD},
     {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
     {FALSE,FALSE}
 };
@@ -87,9 +87,10 @@
 }
 
 // CAN, to revise
-CAN can1(PB_12, PB_13);     // RX, TX
+CAN can1(PB_8, PB_9);     // RX, TX
 
 CANMessage messageIn;
+CANMessage messageOut;
 
 void canrx()
 {
@@ -97,6 +98,8 @@
   {    
     if(can1.read(messageIn))
     {
+      printf("READ!\n\r");
+      
       if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
       {
           speed = 0;
@@ -109,7 +112,19 @@
       }
     }
     
-    wait(0.1);
+    int speed = 5000;
+    
+    messageOut.data[0] = speed >> 24;
+   messageOut.data[1] = speed >> 16;
+   messageOut.data[2] = speed >> 8;
+   messageOut.data[3] = speed;
+   
+   messageOut.id = (3 << 8) + 2;
+    int status = can1.write(messageOut);
+    
+    printf("STATUS: %d\n\r", status);
+    
+    wait(0.01);
   }
 }
 
@@ -117,8 +132,10 @@
 /* Main ----------------------------------------------------------------------*/
 int main()
 {
-  can1.frequency(500000);
+  can1.frequency(125000);
   messageIn.format=CANExtended;
+  messageOut.format=CANExtended;
+  
   
   // Motor Initialization 
   
@@ -146,7 +163,8 @@
   
   printf("DONE: CAN Init\n\r");
   
-
+  //motor->set_speed(1, 50);
+  //motor->run(1, BDCMotor::FWD);
   
   printf("Running!\n\r");