rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Comitter:
Lightvalve
Date:
Mon May 31 07:19:49 2021 +0000
Parent:
231:e00e71ca3e80
Child:
233:2ec0de8590d3
Commit message:
210531

Changed in this revision

CAN/function_CAN.cpp Show annotated file Show diff for this revision Revisions of this file
function_utilities/function_utilities.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/CAN/function_CAN.cpp	Tue Apr 27 11:31:49 2021 +0000
+++ b/CAN/function_CAN.cpp	Mon May 31 07:19:49 2021 +0000
@@ -506,8 +506,8 @@
         }
 
         case CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE: {
-            TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.0001f);
-            spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f));
+            TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.01f);
+            spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*100.0f));
 
             break;
         }
@@ -671,12 +671,12 @@
         int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
 
         if((OPERATING_MODE&0b001)==0) { // Rotary Actuator
-            REF_POSITION = (float)temp_pos * 1.0f / ENC_PULSE_PER_POSITION; // pulse >> deg 
-            REF_VELOCITY = (float)temp_vel * 10.0f / ENC_PULSE_PER_POSITION; // pulse/s >> deg/s 
-            REF_TORQUE = (float)temp_torq * 0.01f / TORQUE_SENSOR_PULSE_PER_TORQUE;  // pulse >> Nm
+            REF_POSITION = (float)temp_pos / 200.0f;
+            REF_VELOCITY = (float)temp_vel / 20.0f;
+            REF_TORQUE = (float)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;  // pulse >> Nm
         } else { //Linear Actuator
-            REF_POSITION = (float)temp_pos * 4.0f / ENC_PULSE_PER_POSITION; // pulse >> mm
-            REF_VELOCITY = (float)temp_vel * 100.0f / ENC_PULSE_PER_POSITION; // pulse/s >> mm/s 
+            REF_POSITION = (float)temp_pos / 200.0f;
+            REF_VELOCITY = (float)temp_vel / 20.0f;
             REF_FORCE = (float)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;  // pulse >> N
         }
 
@@ -1107,7 +1107,7 @@
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
-    int temp_torque_sensor_pulse_per_torque = (int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 10000.0f);
+    int temp_torque_sensor_pulse_per_torque = (int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 100.0f);
     temp_msg.data[1] = (uint8_t) temp_torque_sensor_pulse_per_torque;
     temp_msg.data[2] = (uint8_t) (temp_torque_sensor_pulse_per_torque >> 8);
 
--- a/function_utilities/function_utilities.cpp	Tue Apr 27 11:31:49 2021 +0000
+++ b/function_utilities/function_utilities.cpp	Mon May 31 07:19:49 2021 +0000
@@ -437,7 +437,7 @@
     ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
     STROKE = spi_eeprom_read(RID_STROKE);
     ENC_PULSE_PER_POSITION = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION));
-    TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.0001f;
+    TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.01f;
     PRES_SENSOR_A_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
 //    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
     PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
--- a/main.cpp	Tue Apr 27 11:31:49 2021 +0000
+++ b/main.cpp	Mon May 31 07:19:49 2021 +0000
@@ -238,7 +238,7 @@
     make_delay();
 
     ////// bno rom
-//    spi_eeprom_write(RID_BNO, (int16_t) 4);
+//    spi_eeprom_write(RID_BNO, (int16_t) 2);
 //    make_delay();
     ////////
 
@@ -457,20 +457,20 @@
             float pres_A_new, pres_B_new;
             if (DIR_VALVE_ENC > 0) {
                 pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
-                pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR;
+                pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
             } else {
                 pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
-                pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR;
+                pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
             }
             pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f);
             pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f);
 
             if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
                 float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm 
-                torq.UpdateSen(torq_new,FREQ_TMR4,1000.0);  // unit : Nm
+                torq.UpdateSen(torq_new,FREQ_TMR4,1000.0f);  // unit : Nm
             } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
                 float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
-                force.UpdateSen(force_new,FREQ_TMR4,1000.0);  // unit : N
+                force.UpdateSen(force_new,FREQ_TMR4,1000.0f);  // unit : N
             }
         }
 
@@ -576,7 +576,7 @@
                 static float PresB_pulse_sum = 0.0; 
                 
                 // DAC Voltage reference set
-                float VREF_TuningGain = 0.000003f;
+                float VREF_TuningGain = -0.000003f;
                 if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) {
                     LED = 1;
                     if(SENSING_MODE == 0) { // Force Sensor (Loadcell)
@@ -684,10 +684,16 @@
                     }
                 } else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
                     
-                    int T_move = 2*TMR_FREQ_5k;
-                    REF_POSITION_FINDHOME = (0.0f - REF_POSITION_FINDHOME_INIT)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)REF_POSITION_FINDHOME_INIT;
-
+//                    int T_move = 2*TMR_FREQ_5k;
+                    int T_move = 10000;
+                    REF_POSITION_FINDHOME = ((0.0f - REF_POSITION_FINDHOME_INIT)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)REF_POSITION_FINDHOME_INIT)/ENC_PULSE_PER_POSITION;
+    
                     cnt_findhome++;
+                    
+                    REFERENCE_MODE = MODE_REF_FINDHOME;
+                    CONTROL_MODE = MODE_JOINT_CONTROL;
+                    alpha_trans = 0.0f;
+                    
                     if (cnt_findhome >= T_move) {
                         cnt_findhome = 0;
                         pos.ref = 0.0f;
@@ -1004,9 +1010,11 @@
             // Position, Velocity, and Torque (ID:1200)
             if (flag_data_request[0] == HIGH) {
                 if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
-                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*8.0f));
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+//                    CAN_TX_POSITION_FT((int16_t) (PRES_A_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_A.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
                 } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
-                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*8.0f));
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+//                    CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
                 }
             }