Diff: main.cpp

Revision:

1:a636d553c70a

Parent:

0:c725bf9715c4

Child:

2:042ea0741254

--- a/main.cpp	Thu Jul 14 09:59:39 2022 +0000
+++ b/main.cpp	Thu Jul 21 13:46:47 2022 +0000
@@ -2,125 +2,60 @@
 #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 PULSE_MAX 2450  // us
+#define PULSE_MIN 550   //us  
+#define PERIOD 20       // us
+#define POS_MAX 3.14159265359f  // rad
+#define POS_MIN 0.0f            // rad
+#define M_PI 3.14159265359f  // rad
 
-#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
+#define CAN_ID 4
 
 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);
+
+PwmOut servo1(PA_1);
+PwmOut servo2(PA_3);
 
 ControllerStruct controller;
 
 void moveServo(uint8_t id,double degree);
 void readCANData();
-void offLED();
-void shineLED(uint8_t color_id,double brightness);
 
+bool received = false;
 
 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;
+    servo1.period_ms(PERIOD);
+    servo2.period_ms(PERIOD);
+
     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;
+        if (received == true){
+            double gripper_pos = controller.p_des;
+            int pulse1 = (PULSE_MAX - PULSE_MIN) * gripper_pos / (POS_MAX - POS_MIN) + PULSE_MIN;
+            int pulse2 = (PULSE_MAX - PULSE_MIN) * (M_PI - gripper_pos) / (POS_MAX - POS_MIN) + PULSE_MIN;
+            servo1.pulsewidth_us(pulse1);
+            servo2.pulsewidth_us(pulse2);
+            wait(0.01);
         }
     }
 }
 
-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);
+            received = true;
         }
     }
-    offLED();
-}
+}
\ No newline at end of file