Diff: main.cpp

Revision:

0:c725bf9715c4

Child:

1:a636d553c70a

diff -r 000000000000 -r c725bf9715c4 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 14 09:59:39 2022 +0000
@@ -0,0 +1,126 @@
+#include "mbed.h"
+#include "CAN_com.h"
+#include "structs.h"
+
+#define MAX_PULSE 2300
+#define MIN_PULSE 700
+#define MAX_DEGREE 180.0f
+#define MIN_DEGREE 0.0f
+
+#define GRIPPER_ID 0
+#define JAMMER_ID 1
+
+#define RED_ID 0
+#define BLUE_ID 1
+#define PURPLE_ID 2
+
+
+#ifndef M_PI
+#define M_PI 3.14159265359f
+#endif
+
+//float __float_reg[64];                                                          // Floats stored in flash
+//int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
+
+CAN can(PA_11,PA_12);
+Ticker can_ticker;
+Timer timer;
+PwmOut led_blue(PB_4);
+PwmOut led_red(PB_5);
+PwmOut gripper(PA_1);
+PwmOut jammer(PA_4);
+
+ControllerStruct controller;
+
+void moveServo(uint8_t id,double degree);
+void readCANData();
+void offLED();
+void shineLED(uint8_t color_id,double brightness);
+
+
+int main()
+{
+    //controller.v_bus = 6.0f;    //6[V]
+    //controller.mode = 0;
+    can.frequency(1000000);
+    can.filter(CAN_ID, 0xFFF, CANStandard, 0);
+    //msg.len = 8;
+    can.attach(&readCANData);
+
+    //can_ticker.attach(&readCANData,0.01);
+    timer.start();
+    gripper.period_ms(20);
+    jammer.period_ms(20);
+    int kai=0;
+    double position_now=0;
+    double pre_loop_time=0;
+    while(1) {
+        /*
+        double velocity = controller.kp*(controller.p_des-position_now)+controller.kd*controller.v_des;
+        double loop_time=timer.read();
+        double dt=loop_time-pre_loop_time;
+        position_now += velocity*dt;
+        moveServo(GRIPPER_ID,position_now*M_PI/180.0f);
+        moveServo(JAMMER_ID,position_now*M_PI/180.0f);
+        pre_loop_time=loop_time;
+        */
+        
+        moveServo(GRIPPER_ID,controller.p_des*180.0f/M_PI);
+        moveServo(JAMMER_ID,controller.p_des*180.0f/M_PI);
+        //offLED();
+        kai++;
+        if(kai>1000) {
+            //printf("303:time=%f,position=%f\r\n",loop_time,position_now);
+            printf("p:%f,v:%f,kp:%f,kd:%f\r\n",controller.p_des,controller.v_des,controller.kp,controller.kd);
+            kai=0;
+        }
+    }
+}
+
+void offLED()
+{
+    led_blue=0;
+    led_red=0;
+}
+
+void onLED(uint8_t color_id,double brightness)
+{
+    if(brightness>1)brightness=1;
+    else if(brightness<0)brightness=0;
+    switch(color_id) {
+        case RED_ID:
+            led_blue=0;
+            led_red=brightness;
+            break;
+        case BLUE_ID:
+            led_blue=brightness;
+            led_red=0;
+            break;
+        case PURPLE_ID:
+            led_blue=brightness;
+            led_red=brightness;
+            break;
+        default:
+            break;
+    }
+}
+
+void moveServo(uint8_t id,double degree)
+{
+    int pulse=(MAX_PULSE-MIN_PULSE)*degree/(MAX_DEGREE-MIN_DEGREE)+MIN_PULSE;
+    if(id==0)gripper.pulsewidth_us(pulse);
+    else if(id==1)jammer.pulsewidth_us(pulse);
+}
+
+void readCANData()
+{
+    CANMessage can_msg;
+    //offLED();
+    if(can.read(can_msg)) {
+        if(can_msg.id==CAN_ID) {
+            onLED(BLUE_ID,0.2);
+            unpack_cmd(can_msg,&controller);
+        }
+    }
+    offLED();
+}