Diff: main.cpp

Revision:

1:10f557e7216d

Parent:

0:0da0fd430e20

Child:

2:96ee24b5f266

--- a/main.cpp	Thu Dec 06 14:43:47 2018 +0000
+++ b/main.cpp	Fri Feb 08 15:24:48 2019 +0000
@@ -1,72 +1,133 @@
- #include "mbed.h" //5.4.7 (144)
+#include "mbed.h"
+
+#define MOTOR_ENABLE_PIN PA_8
+#define MOTOR_DIR_PIN PA_9
+
+// Utility
+InterruptIn button(USER_BUTTON, PullUp);
+DigitalOut led(LED1);
+
+// Motor Control
+DigitalOut motor_enable(MOTOR_ENABLE_PIN);
+DigitalOut motor_dir(MOTOR_DIR_PIN);
+
+InterruptIn end0(PC_12, PullUp);
+InterruptIn end1(PC_11, PullUp);
+InterruptIn enc(PC_10, PullUp);
 
-const PinName can1rxPins[] = {PA_11};
-const PinName can1txPins[] = {PA_12};
-const PinName can2rxPins[] = {PB_12};
-const PinName can2txPins[] = {PB_13};
+int current_pose = 0;
+int pose = 0;
+
+void motor_go_fwd()
+{
+    motor_enable = 1;
+    motor_dir = 1;
+}
+
+void motor_go_bwd()
+{
+    motor_enable = 1;
+    motor_dir = 0;
+}
+
+void motor_stop()
+{
+    motor_enable = 0;
+    motor_dir = 0;
+}
 
-//CAN1
-//0,0=OK 0,1=OK 0,2=OK 1,0=OK 1,1=OK 1,2=OK 2,0=OK 2,1=OK 2,2=OK
-//CAN2
-//0,0=RX_OK 0,1=OK 1,0=RX_OK 1,1=OK
+void motor_set_home()
+{
+    printf("END1: Released\n\r");
+    motor_stop();
+    
+    current_pose = 0;
+    pose = 0;
+}
+
+void button_int_handler()
+{
+    printf("BUTTON: Pressed\n\r");
+    motor_go_fwd();
+}
 
-CAN can1(can1rxPins[0], can1txPins[0]);
-CAN can(can2rxPins[0], can2txPins[0]);
+void end0_int_handler()
+{
+  motor_stop();
+  printf("END0: Pressed\n\r");
+  motor_go_fwd();
+}
+
+void end0_released()
+{
+  motor_stop();
+  printf("END0: Released\n\r");
+}
 
-  //UART1, Tx, Rx (Debug)
-DigitalOut led1(LED1);
+void end1_int_handler()
+{
+    motor_stop();
+    printf("END1: Pressed\n\r");
+    motor_go_bwd();
+}
 
+// CAN
+Thread canrxa;
 
-Thread sendThread(osPriorityAboveNormal, 2048);
-Thread canrx;
+CAN can1(PA_11, PA_12);     // RX, TX
+
 CANMessage messageIn;
 CANMessage messageOut;
 
-void canRxIsr()
+int filter = can1.filter(0x030, 0x4FF, CANStandard);
+
+void canrx()
 {
-    while(1)
+  while(1)
+  {
+    if(can1.read(messageIn, filter))
     {
-    if(can1.read(messageIn))
-     {
-      led1 = !led1;
-       printf("received\n\r");
-     }
+      printf("CAN: mess %d\n\r", (messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24)));
+      printf("CANaacc: id %x \n\r ",messageIn.id);
     }
-   }
-void sendMessage()
-{
-    int status = can.write(messageOut);
-   printf("Send status: %d\r\n", status);
+    
+    wait(100);
+  }
 }
 
-void sendMessageLoop()
-{
-    while (true)
-    {
-        sendMessage();
-        osDelay(1000);
-    }
-}
+
+
+/* Main ----------------------------------------------------------------------*/
 
 int main()
 {
-    
-    printf("CAN receive / transmit test.\r\n");
-    //can.frequency(125000);
-
-    messageOut.id = 1337;
-    //messageOut.format = CANExtended;
-    //messageOut.len = 0;
+  // Motor Initialization 
+  motor_stop();
+  
+  end0.rise(&end0_int_handler);
+  end0.fall(&end0_released);
+  end1.rise(&end1_int_handler);
+  end1.fall(&motor_set_home);
+  
+  button.rise(&button_int_handler);
+  
+  printf("DONE: Motor Init\n\r");
+  
+  // CAN Initialization  
+  canrxa.start(canrx);
 
-    canrx.start(canRxIsr);
-      printf("CAN receive / .\r\n");
-    sendThread.start(&sendMessageLoop);
-      printf("CAN \r\n");
+  printf("DONE: CAN Init\n\r");
+  
+  
+  printf("Running!\n\r");
+  
+  motor_enable = 1;
+  motor_dir = 0;
+  
+  while(true)
+  {
+    wait(1000);
+  }
+}
 
-    while (true)
-    {
-        
-        led1 = 0;
-        osDelay(1000);
-    }
-}         
\ No newline at end of file
+