update format

ALGO_AlgorithmBase_ATY.h

@@ -381,6 +381,10 @@ void ALGO_InvertBitsN_Group(uint32_t* genBuf, uint32_t* srcBuf, uint8_t len);
     dataOut /= dataInGroupSize##vN - 2; \
 }
 
+
+// #define SIGMOID(x) (1.0f / (1.0f + exp(-x)))
+
+
 float ALGO_MATH_POW_EASY(float x, uint8_t n);
 int ALGO_MATH_POW_QUICK(int x, int n);
 float ALGO_MATH_POW(float x, int n);

GP22_ATY.c

@@ -266,7 +266,8 @@ uint8_t GP22_CalcHSC(void)
         correctionFactor = ((1 << (GP22_CLKHS_PERIOD + 1)) * 1000 / 32.768)
             / (hscReadValue / 65536) * GP22_CLKHS_FREQ;
         GP22_CALC_CLKHS_FREQ =
-            correctionFactor * GP22_CLKHS_FREQ;
+            GP22_CLKHS_FREQ;
+            // correctionFactor * GP22_CLKHS_FREQ;
 #ifdef __DEBUG_GP22_ATY
         printf("\r\nCHSC - Theoretical value: 0x%08X - %1.6f",
             hscReadValue, (float)hscReadValue / 65536);
@@ -348,11 +349,11 @@ uint32_t PW1ST_ValueA = 0;
 float machineDelayTime = 0;
 float usDistance = 0;
 
-uint8_t pulseGenNum = 3;
-uint8_t pulseNum = 4;
-uint8_t firstWaveEnable = 1;
-uint16_t ignoreTime = 1200;
-uint16_t offsetValue = 0;
+volatile uint8_t pulseGenNum = 3;
+volatile uint8_t pulseNum = 4;
+volatile uint8_t firstWaveEnable = 1;
+volatile uint16_t ignoreTime = 1200;
+volatile uint16_t offsetValue = 0;
 
 
 // #define GP22_MEASURE_US_ANALOG
@@ -364,23 +365,21 @@ uint16_t offsetValue = 0;
 #if defined(GP22_MEASURE_UsWave1MHz)
 void GP22_RegInit_UsWave1MHz(void)
 {
-    // 630BE800
     GP22_REG[0] = GP22_CFG_KEEP_DEFAULT0
         | GP22_CFG0_ANZ_FIRE_0 * pulseGenNum    // 3-6 pulse
-        | GP22_CFG0_DIV_FIRE_0 * 3              // 0CLK | 3FIRE 1MHz pulse
+        | GP22_CFG0_DIV_FIRE_0 * 1              // 0CLK | 4 Div for 1MHz pulse
         | GP22_CFG0_ANZ_PER_CALRES_0 * GP22_CLKHS_PERIOD
         | GP22_CFG0_DIV_CLKHS_0 * GP22_CLKHS_DIV
         | GP22_CFG0_START_CLKHS_0 * 1
         | GP22_CFG0_CALIBRATE * 1
-        // | GP22_CFG0_NO_CAL_AUTO * 1
+        | GP22_CFG0_NO_CAL_AUTO * 0             // 1 to disable, enable is necessary at mode 2
         | GP22_CFG0_MESSB2 * 1
 
         | GP22_CFG0_ANZ_PORT * 1
         | GP22_CFG0_TCYCLE * 1
-        | GP22_CFG0_ANZ_FAKE * 1
+        | GP22_CFG0_ANZ_FAKE * 0
         | GP22_CFG0_SEL_ECLK_TEMP * 1
         | GP22_Ids[0];
-    // 194913F1
     GP22_REG[1] = GP22_CFG_KEEP_DEFAULT1
         | GP22_CFG1_HIT2_0 * 2
         | GP22_CFG1_HIT1_0 * 1
@@ -391,7 +390,6 @@ void GP22_RegInit_UsWave1MHz(void)
         | GP22_CFG1_SEL_TSTO1_0 * 6
         // | GP22_CFG1_EN_FAST_INIT * 1
         | GP22_Ids[1];
-    // A00000F2
     GP22_REG[2] = GP22_CFG_KEEP_DEFAULT2
         | GP22_CFG2_EN_INT_TDC_TIMEOUT * 1
         | GP22_CFG2_EN_INT_HITS * 1
@@ -400,38 +398,37 @@ void GP22_RegInit_UsWave1MHz(void)
         | GP22_CFG2_RFEDGE1 * 0             // 0: rising or falling, 1: and
         | GP22_CFG2_DELVAL1_0 * ignoreTime / (1 << GP22_CLKHS_DIV)
         | GP22_Ids[2];
-    // 000000F3
     GP22_REG[3] = GP22_CFG_KEEP_DEFAULT3
         | GP22_CFG3FW_EN_AUTOCALC_MB2 * 1
         | GP22_CFG3FW_EN_ERR_VAL * 1
         | GP22_CFG3FW_EN_FIRST_WAVE * firstWaveEnable
-        | GP22_CFG3FW_SEL_TIMO_MB2_0 * 3
+        | GP22_CFG3FW_SEL_TIMO_MB2_0 * 1
+        // | GP22_CFG3FW_SEL_TIMO_MB2_0 * 0
         | GP22_CFG3FW_DELREL3_0 * 5
         | GP22_CFG3FW_DELREL2_0 * 4
         | GP22_CFG3FW_DELREL1_0 * 3
         | GP22_Ids[3];
-    // 200000F4
     GP22_REG[4] = GP22_CFG_KEEP_DEFAULT4
         | GP22_CFG4FW_DIS_PW * 1            // 1: disable
         | GP22_CFG4FW_OFFSRNG1 * (((uint16_t)offsetValue % 10) & 0x01)
         | GP22_CFG4FW_OFFSRNG2 * (((uint16_t)offsetValue % 10) & 0x02)
         | GP22_CFG4FW_EDGE_FW * (((uint16_t)offsetValue % 10) & 0x04)
         | GP22_CFG4FW_OFFS_0 * ((uint16_t)offsetValue / 10)
-        // above 4 for first wave mode
+        // above 4 only take effect at first wave enable mode
         | GP22_Ids[4];
-    // 100000F5
     GP22_REG[5] = GP22_CFG_KEEP_DEFAULT5
         | GP22_CFG5_CON_FIRE_DOWN * 1
         | GP22_CFG5_EN_STARTNOISE * 1
         | GP22_CFG5_DIS_PHASESHIFT * 1
         | GP22_Ids[5];
-    // 002000F6
     GP22_REG[6] = GP22_CFG_KEEP_DEFAULT6
         | GP22_CFG6_EN_ANALOG * 1
         | GP22_CFG6_EN_INT_END * 1
-        // | GP22_CFG6_TW2_0 * 3
+        | GP22_CFG0_START_CLKHS_0 * 1
+        | GP22_CFG6_TW2_0 * 3
         | GP22_CFG6_QUAD_RES * 1
-        | GP22_CFG6_DA_KORR_0 * 8
+        // | GP22_CFG6_DOUBLE_RES * 1
+        // | GP22_CFG6_DA_KORR_0 * 8
 
         | GP22_CFG6_NEG_STOP_TEMP * 1       // Must set handly
         // | GP22_CFG6_FIREO_DEF * 1
@@ -478,8 +475,8 @@ uint8_t GP22_StartTof_UsWave1MHz(void)
     waveFreq = 1 / wavePeriod;              // MHz
     echoTimeDetect[0] = ((float)tofValue[0] / GP22_CALC_CLKHS_FREQ / 65536);
         // - (wavePeriod * 2);
-    if(echoTimeDetect[0] <= 0)
-        echoTimeDetect[0] = machineDelayTime + 1;
+    // if(echoTimeDetect[0] <= 0)
+    //     echoTimeDetect[0] = machineDelayTime + 1;
     echoTimeCalc[0] = echoTimeDetect[0] - machineDelayTime;
     speedWave[0] = usDistance * 1000.0 / echoTimeCalc[0];
 
@@ -521,8 +518,10 @@ uint8_t GP22_Process_UsWave1MHz(uint8_t cmdCode)
     }
     // if(cmdCode & GP22_STEP_COMPARE_E2)
     //     if(GP22_CompareE2()) return GP22_STEP_COMPARE_E2;
+
     if(cmdCode & GP22_STEP_CALC_HSC)
         if(GP22_CalcHSC()) return GP22_STEP_CALC_HSC;
+    // GP22_CALC_CLKHS_FREQ = GP22_CLKHS_FREQ;
     if(cmdCode & GP22_STEP_START_TEMP)
         if(GP22_StartTemp()) errCode = GP22_STEP_START_TEMP;
     if(cmdCode & GP22_STEP_START_TOF)
@@ -539,7 +538,7 @@ uint8_t GP22_Process_Run(uint8_t cmdCode)
 #ifdef __DEBUG_GP22_ATY
     printf("\r\nStart");
 #endif /* __DEBUG_GP22_ATY */
-    GP22_WAKE;
+    // GP22_WAKE;
     errCode = GP22_Process_UsWave1MHz(cmdCode);
     // GP22_AnalyseErrCode();
 #ifdef __DEBUG_GP22_ATY

GP22_ATY.h

@@ -55,11 +55,11 @@ extern uint32_t PW1ST_ValueA;
 extern float machineDelayTime;
 extern float usDistance;
 
-extern uint8_t pulseGenNum;
-extern uint8_t firstWaveEnable;
-extern uint8_t pulseNum;
-extern uint16_t ignoreTime;
-extern uint16_t offsetValue;
+extern volatile uint8_t pulseGenNum;
+extern volatile uint8_t firstWaveEnable;
+extern volatile uint8_t pulseNum;
+extern volatile uint16_t ignoreTime;
+extern volatile uint16_t offsetValue;
 /******************************************************************************/
 
 
@@ -69,7 +69,7 @@ extern uint16_t offsetValue;
 The TDC-GP22 does only support the following SPI mode (Motorola specification) *
 Clock Phase Bit = 1 Clock Polarity Bit = 0
 */
-#define GP22_HSC_FREQ                   8                                       // MHz
+#define GP22_HSC_FREQ                   4                                       // MHz
 #define GP22_CLKHS_DIV                  1
 /* The resulting clock after the predivider has to be in the allowed range of
 2 MHz to 8 MHz in single and double resolution and from 2 MHz to 6 MHz in quad resolution */

HW_ADC_ATY.c

@@ -43,8 +43,8 @@ void ADC_Init(void)
     P_SW2 |= 0x80;              // change reg sfr
     ADCTIM = 0x3F;              // set ADC internal timing sequence
     P_SW2 &= 0x7F;              // stop change reg sfr
-    ADCCFG = 0x0F;              // set SYSCLK/2/16 as ADC timer
-    // ADCCFG = 0x00;              // set SYSCLK/2/1 as ADC timer
+    // ADCCFG = 0x0F;              // set SYSCLK/2/16 as ADC timer
+    ADCCFG = 0x00;              // set SYSCLK/2/1 as ADC timer
     ADC_CONTR = 0x80;           // enable ADC
 }
 #elif defined(__STM32_HAL_ATY)

HW_PWM_ATY.c

@@ -136,62 +136,77 @@ void PCA_Isr() interrupt 7
  */
 void PWM_Start(uint32_t periodS, float dutyS, uint8_t channelS)
 {
-    if(channelS == 2)
-    {
-        P_SW2 |= 0x80;
-        PWMA_CCER1 = 0x00;
+    P_SW2 |= 0x80;
+    if(channelS == PWM_CHANNEL_1P || channelS == PWM_CHANNEL_1N){
+        PWMA_PS = (PWMA_PS & 0xFC) | (PWM_PIN_POS_1 << 0);      //
+        PWMA_CCER1 &= 0xF0;
+        PWMA_CCMR1 = 0x60;
+        PWMA_CCER1 |= 0x05;
+        PWMA_CCR1 = dutyS / 100.0 * periodS;
+    }
+    else if(channelS == PWM_CHANNEL_2P || channelS == PWM_CHANNEL_2N){
+        PWMA_PS = (PWMA_PS & 0xF3) | (PWM_PIN_POS_1 << 2);      //
+        PWMA_CCER1 &= 0x0F;
         PWMA_CCMR2 = 0x60;
-        PWMA_CCER1 = 0x50;
+        PWMA_CCER1 |= 0x50;
         PWMA_CCR2 = dutyS / 100.0 * periodS;
-        PWMA_ARR = periodS;
-        PWMA_ENO = 0x08;
-        PWMA_PS = (PWMA_PS & 0xF3) | 0x04;      // PWM2N_2 TMC_STEP
-        PWMA_BKR = 0x80;
-        PWMA_CR1 = 0x01;
-        P_SW2 &= 0x7F;
     }
-    else if(channelS == 3)
-    {
-        P_SW2 |= 0x80;
-        PWMA_CCER2 = 0x00;
+    else if(channelS == PWM_CHANNEL_3P || channelS == PWM_CHANNEL_3N){
+        PWMA_PS = (PWMA_PS & 0xCF) | (PWM_PIN_POS_1 << 4);      // PWM3N_2 Heat
+        PWMA_CCER2 &= 0xF0;
         PWMA_CCMR3 = 0x60;
-        PWMA_CCER2 = 0x05;
+        PWMA_CCER2 |= 0x05;
         PWMA_CCR3 = dutyS / 100.0 * periodS;
+    }
+    else if(channelS == PWM_CHANNEL_4P || channelS == PWM_CHANNEL_4N){
+        PWMA_PS = (PWMA_PS & 0x3F) | (PWM_PIN_POS_1 << 6);      //
+        PWMA_CCER2 &= 0x0F;
+        PWMA_CCMR4 = 0x60;
+        PWMA_CCER2 |= 0x50;
+        PWMA_CCR4 = dutyS / 100.0 * periodS;
+    }
+    else if(channelS == PWM_CHANNEL_5P){
+        PWMB_PS = (PWMB_PS & 0xFC) | (PWM_PIN_POS_1 << 0);      //
+        PWMB_CCER1 &= 0xF0;
+        PWMB_CCMR1 = 0x60;
+        PWMB_CCER1 |= 0x01;
+        PWMB_CCR5 = dutyS / 100.0 * periodS;
+    }
+    else if(channelS == PWM_CHANNEL_6P){
+        PWMB_PS = (PWMB_PS & 0xF3) | (PWM_PIN_POS_1 << 2);      // PWM6_2 DAC_PWM P54
+        PWMB_CCER1 &= 0x0F;                                     // Set 0 of CCERx before write CCMRx
+        PWMB_CCMR2 = 0x60;                                      // Set CC6 PWMB output mode
+        PWMB_CCER1 |= 0x10;                                     // Enable CC6 channel
+        PWMB_CCR6 = dutyS / 100.0 * periodS;                    // Set dutys time
+    }
+    else if(channelS == PWM_CHANNEL_7P){
+        PWMB_PS = (PWMB_PS & 0xCF) | (PWM_PIN_POS_2 << 4);      //
+        PWMB_CCER2 &= 0xF0;
+        PWMB_CCMR3 = 0x60;
+        PWMB_CCER2 |= 0x01;
+        PWMB_CCR7 = dutyS / 100.0 * periodS;
+    }
+    else if(channelS == PWM_CHANNEL_8P){
+        PWMB_PS = (PWMB_PS & 0x3F) | (PWM_PIN_POS_0 << 6);      // PWM8 23 TMC_STEP
+        PWMB_CCER2 &= 0x0F;
+        PWMB_CCMR4 = 0x60;
+        PWMB_CCER2 |= 0x10;
+        PWMB_CCR8 = dutyS / 100.0 * periodS;
+    }
+
+    if((channelS & 0xF0) == 0xF0){                              // channel 5-8
+        PWMB_ARR = periodS;                                     // Set period time
+        PWMB_ENO |= ((uint8_t)1 << ((channelS & 0x0F) * 2));    // Enable channel output
+        PWMB_BKR = 0x80;                                        // Enable main output
+        PWMB_CR1 = 0x01;                                        // Start counter
+    }
+    else{                                                       // channel 1-4
         PWMA_ARR = periodS;
-        PWMA_ENO = 0x20;
-        PWMA_PS = (PWMA_PS & 0xCF) | 0x10;      // PWM3N_2 Heat
+        PWMA_ENO |= channelS;
         PWMA_BKR = 0x80;
         PWMA_CR1 = 0x01;
-        P_SW2 &= 0x7F;
-    }
-    else if(channelS == 6)
-    {
-        P_SW2 |= 0x80;
-        PWMB_CCER1 = 0x00;                      // Set 0 of CCERx before write CCMRx
-        PWMB_CCMR2 = 0x60;                      // Set CC6 PWMB output mode
-        PWMB_CCER1 = 0x10;                      // Enable CC6 channel
-        PWMB_CCR6 = dutyS / 100.0 * periodS;    // Set dutys time
-        PWMB_ARR = periodS;                     // Set period time
-        PWMB_ENO = 0x04;                        // Enable PWM6P output
-        PWMB_PS = (PWMB_PS & 0xF3) | 0x04;      // PWM6_2 DAC_PWM P54
-        PWMB_BKR = 0x80;                        // Enable main output
-        PWMB_CR1 = 0x01;                        // Start counter
-        P_SW2 &= 0x7F;
-    }
-    else if(channelS == 8)
-    {
-        P_SW2 |= 0x80;
-        PWMB_CCER1 = 0x00;                      // Set 0 of CCERx before write CCMRx
-        PWMB_CCMR4 = 0x60;                      // Set CC8 PWMB output mode
-        PWMB_CCER2 = 0x10;                      // Enable CC8 channel
-        PWMB_CCR8 = dutyS / 100.0 * periodS;    // Set dutys time
-        PWMB_ARR = periodS;                     // Set period time
-        PWMB_ENO = 0x40;                        // Enable PWM8P output
-        PWMB_PS = (PWMB_PS & 0xF3) | 0x04;      // PWM8 23
-        PWMB_BKR = 0x80;                        // Enable main output
-        PWMB_CR1 = 0x01;                        // Start counter
-        P_SW2 &= 0x7F;
     }
+    P_SW2 &= 0x7F;
 }
 
 /**

HW_PWM_ATY.h

@@ -32,10 +32,36 @@
 
 /******************************* For user *************************************/
 // #define __DEBUG_HW_PWM_ATY
+#define PWM1_CHOSE PWM_PIN_POS_0
+#define PWM2_CHOSE PWM_PIN_POS_0
+#define PWM3_CHOSE PWM_PIN_POS_0
+#define PWM4_CHOSE PWM_PIN_POS_0
+#define PWM5_CHOSE PWM_PIN_POS_0
+#define PWM6_CHOSE PWM_PIN_POS_0
+#define PWM7_CHOSE PWM_PIN_POS_0
+#define PWM8_CHOSE PWM_PIN_POS_0
 
 /******************************************************************************/
 
 #if defined(__STC51_ATY)
+#define PWM_CHANNEL_1P 0x01
+#define PWM_CHANNEL_1N 0x02
+#define PWM_CHANNEL_2P 0x04
+#define PWM_CHANNEL_2N 0x08
+#define PWM_CHANNEL_3P 0x10
+#define PWM_CHANNEL_3N 0x20
+#define PWM_CHANNEL_4P 0x40
+#define PWM_CHANNEL_4N 0x80
+#define PWM_CHANNEL_5P 0xF0
+#define PWM_CHANNEL_6P 0xF1
+#define PWM_CHANNEL_7P 0xF2
+#define PWM_CHANNEL_8P 0xF3
+
+#define PWM_PIN_POS_0 0x00      // PWMN
+#define PWM_PIN_POS_1 0x01      // PWMN_2
+#define PWM_PIN_POS_2 0x02      // PWMN_3
+#define PWM_PIN_POS_3 0x03      // PWMN_4
+
 #elif defined(__STM32_HAL_ATY)
 #elif defined(__ESP8266_RTOS_ATY)
 #endif /* PLATFORM */

MOTOR_STEP_ATY.c

@@ -50,6 +50,14 @@ uint32_t motorStopCounter = 0;      // for stall detection or driver error(TMC22
 
 uint8_t motorSetState = 0;          // 0: stop, 1: start hold, 2: start soft, 3: run speed, 4: change dir, 5: stop state, soft set 0 after use, 10: scram motor
 
+float speedIncreaseStep = 0.0f;
+float S_paramA = 0.0f;
+#ifdef S_PARAM_C
+float S_paramC = S_PARAM_C;
+#else
+float S_paramC = 0.0f;
+#endif
+
 
 /**
  * @brief   start motor with direction set
@@ -116,6 +124,7 @@ void MotorSelfCycle(void)
     }
 }
 
+
 /**
  * @brief   deal step motor state
  * @note    put at 1ms cycle;
@@ -129,6 +138,9 @@ void MotorSelfCycle(void)
  */
 void MotorStateMachine_Step(void)
 {
+    if(motorSoftSpeed < motorSpeed)
+        motorSoftSpeed = motorSpeed;
+
     if(motorSetState == 4){
         motorStartCounter = 0;
         motorStopCounter = 0;
@@ -146,6 +158,45 @@ void MotorStateMachine_Step(void)
         MotorStart(motorDir, motorSoftSpeed);
     }
     else if(motorSetState == 1){
+        if(motorStartCounter < motorSoftTime){
+#if defined (Motor_StartLine_T)
+            speedIncreaseStep = ((float)(motorSoftSpeed - motorSpeed) / (float)motorSoftTime);  // at 1ms cycle
+            MotorStart(motorDir,
+                motorSpeed < (motorSoftSpeed - (speedIncreaseStep * motorStartCounter)) ?
+                (motorSoftSpeed - (speedIncreaseStep * motorStartCounter)) : motorSpeed);
+#elif defined (Motor_StartLine_S)
+            // S line: y = ax^3 + cx, a in 0-1, c >= 0, lower c cause lower increase at start or end, c for users
+            S_paramA = (((float)(motorSoftSpeed - motorSpeed) / 2)
+                - (S_paramC * (float)((float)motorSoftTime / 2) * (float)((float)motorSoftTime / 2) / 2))
+                / ((float)((float)motorSoftTime / 2) * (float)((float)motorSoftTime / 2)
+                    * (float)((float)motorSoftTime / 2) * (float)((float)motorSoftTime / 2)) * 4;
+            // mbG[27] = S_paramA * 10000000;
+            if(motorStartCounter < (motorSoftTime / 2))
+                speedIncreaseStep =
+                ((float)(S_paramA * motorStartCounter * motorStartCounter * motorStartCounter * motorStartCounter) / 4)
+                + ((float)(S_paramC * motorStartCounter * motorStartCounter) / 2);
+            // (S_paramA * motorStartCounter * motorStartCounter * motorStartCounter)
+            // + (S_paramC * motorStartCounter);
+            else
+                speedIncreaseStep =
+                (float)((float)(motorSoftSpeed - motorSpeed) / 2)
+                + ((float)(S_paramA * motorSoftTime * motorSoftTime * motorSoftTime * motorSoftTime) / 64)
+                + ((float)(S_paramC * motorSoftTime * motorSoftTime) / 8)
+                - ((float)(S_paramA * (motorSoftTime - motorStartCounter) * (motorSoftTime - motorStartCounter)
+                    * (motorSoftTime - motorStartCounter) * (motorSoftTime - motorStartCounter)) / 4)
+                - ((float)(S_paramC * (motorSoftTime - motorStartCounter) * (motorSoftTime - motorStartCounter)) / 2);
+                // 0
+                // - (S_paramA * (float)((float)motorStartCounter - (float)motorSoftTime)
+                //     * (float)((float)motorStartCounter - (float)motorSoftTime)
+                //     * (float)((float)motorStartCounter - (float)motorSoftTime))
+                // - (S_paramC * (float)((float)motorStartCounter - (float)motorSoftTime));
+            MotorStart(motorDir,
+                motorSpeed < (motorSoftSpeed - speedIncreaseStep) ?
+                (motorSoftSpeed - speedIncreaseStep) : motorSpeed);
+#else
+#endif
+        }
+
         if(motorStartCounter == motorSoftTime)
             motorSetState = 3;
         else if(motorStartCounter == (motorSoftTime + motorTime))

MOTOR_STEP_ATY.h

@@ -38,7 +38,11 @@
 
 /******************************* For user *************************************/
 // #define __DEBUG_MOTOR_STEP_ATY
-#define Motor_Channel 8
+// #define S_PARAM_C 0.5f      // 0-1
+// #define Motor_StartLine_2
+#define Motor_StartLine_T
+// #define Motor_StartLine_S
+#define Motor_Channel PWM_CHANNEL_8P
 
 /******************************************************************************/