Revision 32:d83629b32b10, committed 2019-09-15

Comitter:

s242743

Date:

Sun Sep 15 02:17:24 2019 +0000

Parent:

31:9d89620d794b

Commit message:

.

Changed in this revision

diff -r 9d89620d794b -r d83629b32b10 main.cpp
--- a/main.cpp	Sat Sep 14 22:44:08 2019 +0000
+++ b/main.cpp	Sun Sep 15 02:17:24 2019 +0000
@@ -1,52 +1,12 @@
 #include "mbed.h"
 #include "L6206.h"
 #include "BDCMotor.h"
-#include <math.h>
-
-/****************************/
-/*      PIN DEFINITION      */
-/****************************/
-
-//  CAN
-#define CAN_RX  PB_8
-#define CAN_TX  PB_9
-
-//  ENCODER
-#define CH_A    PA_8
-#define CH_B    PA_9
-
-/********************************/
-/*             CAN              */
-/********************************/
 
-typedef enum
-{
-    JOINT_SET_SPEED = 20,
-    JOINT_SET_POSITION,
-    JOINT_CURRENT_POSITION,
-    JOINT_CURRENT_SPEED,
-    JOINT_STATUS,
-    JOINT_ERROR,
-    JOINT_TORQUE,
-    JOINT_MAXTORQUE,
-    JOINT_ZERO,
-}   CAN_COMMANDS;
+#define TX PC_10
+#define RX PC_11
 
-typedef enum
-{
-    BASE = 1,
-    SHOULDER,
-    ELBOW,
-    WRIST1,
-    WRIST2,
-    WRIST3,
-    END_EFFECTOR,
-    CAMERA1,
-    CAMERA2,
-}   JOINT;
 
-//
-DigitalOut myled(LED1);
+Serial SERIAL(D1, D0)
 
 static volatile uint16_t gLastError;
 static volatile uint8_t gStep = 0;
@@ -113,73 +73,11 @@
     }  
 }
 
-/****************************/
-/*      CAN Interface       */
-/****************************/
-
-CAN can1(PB_8, PB_9);     // RX, TX
-
-Thread t_canrx, t_cantx;
+/*****************************************************************************/
 
-uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
-{
-    uint32_t id = (uint32_t)can_id;     // LSB byte is the controller id.
-    id |= (uint32_t)message_id << 8;  // Next lowest byte is the packet id.
-    id |= 0x80000000;              // Send in Extended Frame Format.
-    return id;
-}
-
-bool can_rx()
+void rx()
 {
-    CANMessage messageIn;
-    messageIn.format = CANExtended;
-    bool status = can1.read(messageIn);
-    printf ("CAN ID : %d Message received : %d\n\r", messageIn.id, status);
-    
-    if(can1.read(messageIn))
-    {
-        myled = 1;
-        wait(0.5);
-        myled = 0;
-        wait(0.5);
-        if(messageIn.id == gen_can_id(JOINT_SET_SPEED, ELBOW))
-        {
-            speed_elbow = messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
-        }
-    }
-    
-    /*
-    if(can1.read(messageIn))
-    {
-        myled = 1;
-        wait(0.5);
-        myled = 0;
-        wait(0.5);
-        if(messageIn.id == gen_can_id(JOINT_SET_SPEED, END_EFFECTOR))
-        {
-            speed_ee = messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
-        }
-    }
-    */
-    
-    if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_ZERO,ELBOW))
-    {
-        if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24)) == 1)
-        {
-            LinAct->run(1, BDCMotor::BWD);
-        }
-    }
-    
-    return status;
-}
-
-void can_rx_isr()
-{
-    while(1)
-    {
-        can_rx();
-        osDelay(10);
-    }
+        
 }
 
 /*****************************/
@@ -188,7 +86,6 @@
 
 int main()
 {
-    can1.frequency(125000);
 
 //  Motor Initialization   
 #ifdef TARGET_STM32F429
@@ -220,10 +117,6 @@
     LinAct->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B);
     EndEff->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B);
 
-    // CAN Initialization
-    t_canrx.start(can_rx_isr);
-    printf("DONE: CAN Init\n\r");
-
     while(true)
     {            
         EndEff->set_speed(0, speed_ee);