Diff: main.cpp

Revision:

27:275ba9c137c9

Parent:

26:44175c51a820

Child:

28:8878dd50b7e1

--- a/main.cpp	Tue Jun 18 16:28:38 2019 +0000
+++ b/main.cpp	Fri Jun 21 07:44:12 2019 +0000
@@ -6,7 +6,7 @@
 
 #define JOINT_SET_SPEED 20
 
-#define JOINT_ID 1
+#define JOINT_ID 2
 
 l6208_init_t init =
 {
@@ -33,8 +33,8 @@
 // Motor Control
 L6208 *motor;
 
-InterruptIn end0(PC_10, PullUp);
-InterruptIn end1(PC_11, PullUp);
+InterruptIn end0(PC_4, PullUp);
+InterruptIn end1(PC_8, PullUp);
 InterruptIn enc(PC_12, PullUp);
 
 int32_t speed = 0;
@@ -58,13 +58,14 @@
 
 void end0_int_handler()
 {
+    motor->run(StepperMotor::BWD);
     printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
 }
 
 void end1_int_handler()
 {
     motor->hard_stop();
-    motor->run(StepperMotor::BWD);
+    motor->run(StepperMotor::FWD);
 
     printf("END1: Pressed\n\r");
 }
@@ -77,7 +78,7 @@
 }
 
 // CAN
-CAN can1(PB_12, PB_13);     // RX, TX
+CAN can1(PB_8, PB_9);     // RX, TX
 
 CANMessage messageIn;
 CANMessage messageOut;
@@ -88,6 +89,7 @@
   {    
     if(can1.read(messageIn))
     {
+      printf("received\r\n");
       if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
       {
           speed = 0;
@@ -100,7 +102,7 @@
       }
     }
     
-    wait(0.1);
+    wait(0.01);
   }
 }
 
@@ -109,7 +111,8 @@
 
 int main()
 {
-  can1.frequency(500000);
+  can1.frequency(125000);
+  messageIn.format=CANExtended;
   
   // Motor Initialization 
   motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
@@ -139,6 +142,10 @@
   
   printf("Running!\n\r");
   
+  // DEBUG
+  //motor->set_max_speed(8000);
+  //motor->run(StepperMotor::FWD);
+  
   while(true)
   {
     wait(1);