Luca Antolini
All the previous but the PID
Dependencies: mbed QEI PID DmTftLibraryEx
Diff: main.cpp
- Revision:
- 28:443c82750dd9
- Parent:
- 27:654100855f5c
- Child:
- 29:7ce8c5779f5e
--- a/main.cpp Tue Feb 15 07:22:09 2022 +0000
+++ b/main.cpp Fri Mar 04 14:03:57 2022 +0000
@@ -1,3 +1,4 @@
+
//Warning: Incompatible redefinition of macro "MBED_RAM_SIZE" in "tmp/HU5Hqj", Line: 39, Col: 10
#ifndef MBED_RAM_SIZE
@@ -54,12 +55,15 @@
//
PwmOut PWM_PB3(PWM_OUT); // LA: PWM_OUT = D3 = PB_3
+// QEI
+// ===
+//
+QEI Stabilus322699 (PA_1, PA_0, NC, 100, QEI::X4_ENCODING);
+
// Motion
// ======
//
-//Ticker POS_MotionScan; //
-
-QEI Stabilus322699 (PA_1, PA_0, NC, 100, QEI::X4_ENCODING);
+//Ticker POS_MotionScan; // LA: Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM
//
in_sPosizionatoreSW in_PosizionatoreSW;
out_sPosizionatoreSW out_PosizionatoreSW;
@@ -67,26 +71,13 @@
// LCD Display
// ===========
//
-//Ticker LCD_RefreshViews; //
-
-//extern void StabilusMenu (uint32_t, uint32_t, int64_t); // LA: Prototype ...
+//Ticker LCD_RefreshViews; // LA: Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM
void FactoryReset (void) {
EepromFactoryReset( );
HAL_NVIC_SystemReset( );
}
-/*
-void LCD____tScan (void) {
-// StabilusMenu (out_PosizionatoreSW.ui32_PreviousStep_ms, out_PosizionatoreSW.ui32_ActualStepSampled_ms, out_PosizionatoreSW.ui32_PassedActual_ms);
-}
-
-
-void Motion_tScan (void) {
- PosizionatoreSW (in_PosizionatoreSW, out_PosizionatoreSW);
-}
-*/
-
// =======
// =======
// Main(s)
@@ -94,10 +85,21 @@
// =======
//
int main (void){
-//int32_t i32_Pulses;
-// F_CS = 1; // LA: HW Initialization InProgress
-// SD_CS = 1; // LA: HW Initialization InProgress
-// BoardLED3 = 1;
+char StringText[MAX_CHAR_PER_LINE + 1]; // don't forget the /0 (end of string)
+//char StringText2[MAX_CHAR_PER_LINE + 1];
+//char StringText3[MAX_CHAR_PER_LINE + 1];
+
+//uint16_t ui16_TestColor = 0x0000;
+//uint16_t ui16_TestStep = 0x0000;
+//
+//uint16_t ui16_R = 0x00;
+//uint16_t ui16_G = 0x00;
+//uint16_t ui16_B = 0x00;
+
+uint32_t ui32_PreCallms;
+uint32_t ui32_PostCallms;
+uint32_t ui32_Samplems;
+
EepromInit(); // LA: Inizializza la EEProm
TimersInit(); // LA: Parte il Timer a 1ms
@@ -107,20 +109,15 @@
if (userButton == 0) {
FactoryReset();
}
+ DisplayDriverInit();
- DisplayDriverInit();
-// LCM_SetTextColor(BLUE, BLACK);
-// LCM_SetTextColor(GRAY1, BLACK);
-// LCM_SetTextColor(GRAY2, WHITE);
- LCM_SetTextColor(BLACK, WHITE);
-
-// MenuInit(); // LA: Inizializza il Display
-// LCD_RefreshViews.attach_us (&LCD____tScan, 250000); // Scansione LCD @100ms
+ PWM_PB3.period_us(100); // LA: Avvia il PWM con TimeBase 100us
+ PWM_PB3.pulsewidth_us(0); // 0.. 100us -> 0.. 100%, Set to ZERO
// LA: Motion (1st) Setup
//
- in_PosizionatoreSW.b_AxisPowered = false;
- in_PosizionatoreSW.b_ACPos_Homed = false;
+ in_PosizionatoreSW.b_AxisPowered = true;
+ in_PosizionatoreSW.b_ACPos_Homed = true;
in_PosizionatoreSW.i32_Max_Speed = 1024; // [ui]
in_PosizionatoreSW.i32_ZeroSpeed = 0; //
@@ -133,84 +130,140 @@
in_PosizionatoreSW.f_JogSpeed_x100_FW = 25.0; // % of "i32_Max_Speed"
in_PosizionatoreSW.f_JogSpeed_x100_BW = 25.0; //
- in_PosizionatoreSW.b_ServoLock = false;
+ in_PosizionatoreSW.b_ServoLock = true;
in_PosizionatoreSW.rtServoLock_Q = false;
//
- in_PosizionatoreSW.i64_TargetPosition = 0; // [ui]
- in_PosizionatoreSW.i64_ActualPosition = 0; //
- in_PosizionatoreSW.i64_AccelerationWindow = 0; // LA: Spazio concesso all'accelerazione.
- in_PosizionatoreSW.i64_DecelerationWindow = 0; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
- in_PosizionatoreSW.i64_diToleranceWindow = 0; // Finestra di Tolleranza
+ in_PosizionatoreSW.i64_TargetPosition = 1000; // [ui]
+ in_PosizionatoreSW.i64_ActualPosition = 200; //
+ in_PosizionatoreSW.i64_AccelerationWindow = 50; // LA: Spazio concesso all'accelerazione.
+ in_PosizionatoreSW.i64_DecelerationWindow = 50; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
+ in_PosizionatoreSW.i64_diToleranceWindow = 10; // Finestra di Tolleranza
//
in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 25.0; // % of "i32_Max_Speed"
in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 25.0; //
in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 5.0; //
in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 5.0; //
-// POS_MotionScan.attach_us (&Motion_tScan, 2000); // LA: Scansione Motion @2ms
-
-// PWM_SetOut.attach_us (&PWM____tScan, 100); // Scansione base PWM @100us Ok
-// PWM_SetOut.attach_us (&PWM____tScan, 50); // Scansione base PWM @50us Ok
-// PWM_SetOut.attach_us (&PWM____tScan, 30); // Scansione base PWM @30us Ok
-// PWM_SetOut.attach_us (&PWM____tScan, 25); // Scansione base PWM @25us No (al pelo ...)
-// PWM_SetOut.attach_us (&PWM____tScan, 10); // Scansione base PWM @10us No (al pelo ...)
-// PWM_SetOut.attach_us (&PWM____tScan, 20); // Scansione base PWM @20us No
-
-// PWM_SetOut.attach_us (&PWM____tScan, 40); // Scansione base PWM @40us, Duty Cycle 4ms, 0- 100%
-// PWM_SetOut.attach_us (&PWM____tScan, 100); // Scansione base PWM @40us, Duty Cycle 5ms, 0- 100%
+ // LA: Color RGB Component(s)
+ // ======================
+ //
+ // RED 0000 1000 0000 0000 min 0x0800 02048
+ // 1111 1000 0000 0000 max 0xf800 63488
+ //
+ // GREEN 0000 0000 0010 0000 min 0x0020 00032
+ // 0000 0111 1110 0000 max 0x07e0 02016
+ //
+ // BLUE 0000 0000 0000 0001 min 0x0001 00001
+ // 0000 0000 0001 1111 max 0x001f 00031
+ //
+ // La componente ROSSA ha 5 bit di escursione (0.. 31),
+ // La componente VERDE ha 6 bit di escursione (0.. 63),
+ // La componente BLU ha 5 bit di escursione (0.. 31),
+ //
+ // Le componenti RGB di "Color" sono quindi scritte negli appropriati registri come segue:
+ //
+ // writeReg(RED, (Color & 0xf800) >> 11);
+ // writeReg(GREEN, (Color & 0x07e0) >> 5);
+ // writeReg(BLUE, (Color & 0x001f));
+ //
+ LCM_SetTextColor(Scale2RGBColor (31, 31, 31), Scale2RGBColor (0, 0, 0)); // LA: Black on White
+ LCM_ClearScreen (Scale2RGBColor (31, 31, 31));
- PWM_PB3.period_us(100); // LA: TimeBase is 100us
+// ui16_TestColor = Scale2RGBColor (ui16_R, ui16_G, ui16_B);
+// LCM_SetTextColor (
+// Scale2RGBColor (0, 0, 0),
+// Scale2RGBColor (31, 31, 31)
+// );
+ LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 0), "You Start Me Up ...");
+
+ while (1) {
+ static int32_t Pulses_Prec;
+ static uint32_t ms_0002_prec;
+ static uint32_t ms_0003_prec;
+ int32_t i32_Pulses;
- #define RED 0xf800 // 1111 1000 0000 0000
- #define GREEN 0x07e0 // 0000 0111 1110 0000
- #define BLUE 0x001f // 0000 0000 0001 1111
- #define BLACK 0x0000 // 0000 0000 0000 0000
- #define YELLOW 0xffe0 // 1111 1111 1110 0000
- #define WHITE 0xffff // 1111 1111 1111 1111
+ float f_ai0000_Aux;
+ float f_ai0000_prec;
+ float f_ai0001_prec;
+ float f_ai0002_prec;
+
+ i32_Pulses = Stabilus322699.getPulses();
+ PWM_PB3.pulsewidth_us(((float)i32_Pulses/ (float)5000.0)* (float)100.0); // 0.. 100us -> 0.. 100%
+
+ in_PosizionatoreSW.i64_ActualPosition = (int64_t) i32_Pulses; //
- #define CYAN 0x07ff // 0000 0111 1111 1111
- #define BRIGHT_RED 0xf810 // 1111 1000 0001 0000
- #define GRAY1 0x8410 // 1000 0100 0001 0000
- #define GRAY2 0x4208 // 0100 0010 0000 1000
+ ui32_PreCallms = TimersTimerValue(); // Freezes the Actual Sample.
+ PosizionatoreSW (in_PosizionatoreSW, out_PosizionatoreSW);
+ ui32_PostCallms = TimersTimerValue();
+ //
+ if (ui32_PostCallms >= ui32_PreCallms)
+ ui32_Samplems = ui32_PostCallms- ui32_PreCallms; // Result => Actual - Previous
+ else
+ ui32_Samplems = ui32_PostCallms+ (0x7fffffff- ui32_PreCallms); // Result => Actual+ (Rollover- Previous)
+
-// LA: Color RGB Component(s)
-// ======================
-//
-// RED 0000 1000 0000 0000 min 0x0800 02048
-// 1111 1000 0000 0000 max 0xf800 63488
-//
-// GREEN 0000 0000 0010 0000 min 0x0020 00032
-// 0000 0111 1110 0000 max 0x07e0 02016
-//
-// BLUE 0000 0000 0000 0001 min 0x0001 00001
-// 0000 0000 0001 1111 max 0x001f 00031
-//
-// La componente ROSSA ha 5 bit di escursione (0.. 31),
-// La componente VERDE ha 6 bit di escursione (0.. 63),
-// La componente BLU ha 5 bit di escursione (0.. 31),
-//
-// Le componenti RGB di "Color" sono quindi scritte negli appropriati registri come segue:
-//
-// writeReg(RED, (Color & 0xf800) >> 11);
-// writeReg(GREEN, (Color & 0x07e0) >> 5);
-// writeReg(BLUE, (Color & 0x001f));
-//
+ // LA: Wedge 4 LCDRefresh
+ if (
+ (i32_Pulses != Pulses_Prec)
+ ) {
+ sprintf (StringText,
+ "Pulses: %d ", i32_Pulses);
+ LCM_SetTextColor (Scale2RGBColor (0, 0, 31), Scale2RGBColor (31, 31, 0));
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 1), StringText);
+ }
+
+// if (out_PosizionatoreSW.ui32_PassedActual_ms != ms_0003_prec) {
+ if (ui32_Samplems != ms_0003_prec) {
+ sprintf (StringText,
+// "PassedActual_ms: %d ", out_PosizionatoreSW.ui32_PassedActual_ms);
+ "PassedActual_ms: %d ", ui32_Samplems);
+ LCM_SetTextColor (Scale2RGBColor (0, 31, 0), Scale2RGBColor (31, 0, 31));
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 2), StringText);
+// ms_0003_prec = out_PosizionatoreSW.ui32_PassedActual_ms;
+ ms_0003_prec = ui32_Samplems;
+ }
+
+ if (out_PosizionatoreSW.i32_ATVSpeed != ms_0002_prec) {
+ sprintf (StringText,
+ "Speed[ui]: %d ", out_PosizionatoreSW.i32_ATVSpeed);
+ LCM_SetTextColor (Scale2RGBColor (0, 31, 0), Scale2RGBColor (31, 0, 31));
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
+ ms_0002_prec = out_PosizionatoreSW.i32_ATVSpeed;
+ }
-// LCM_ClearScreen (GRAY1);
-// LCM_ClearScreen (BLACK);
-// LCM_ClearScreen (GRAY2);
-
- char StringText[MAX_CHAR_PER_LINE + 1]; // don't forget the /0 (end of string)
-// char StringText2[MAX_CHAR_PER_LINE + 1];
-// char StringText3[MAX_CHAR_PER_LINE + 1];
+ f_ai0000_Aux = adc_temp.read();
+ if (f_ai0000_Aux != f_ai0000_prec) {
+ sprintf (StringText,
+// "ADC Temp = %f\n", (f_ai0000_Aux* 100));
+ "ADC Temp = %f ", (f_ai0000_Aux* 100));
+ LCM_SetTextColor (Scale2RGBColor (0, 31, 0), Scale2RGBColor (31, 0, 31));
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
+ f_ai0000_prec = f_ai0000_Aux;
+ }
- uint16_t ui16_TestColor = 0x0000;
- uint16_t ui16_TestStep = 0x0000;
- //
- uint16_t ui16_R = 0x00;
- uint16_t ui16_G = 0x00;
- uint16_t ui16_B = 0x00;
+ f_ai0000_Aux = adc_vbat.read();
+ if (f_ai0000_Aux != f_ai0001_prec) {
+ sprintf (StringText,
+// "ADC VBat = %f\n", (f_ai0000_Aux* 100));
+ "ADC VBat = %f ", (f_ai0000_Aux));
+ LCM_SetTextColor (Scale2RGBColor (0, 31, 0), Scale2RGBColor (31, 0, 31));
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
+ f_ai0001_prec = f_ai0000_Aux;
+ }
+ f_ai0000_Aux = adc_vref.read();
+ if (f_ai0000_Aux != f_ai0002_prec) {
+ sprintf (StringText,
+// "ADC VRef = %f\n", (f_ai0000_Aux* 100));
+ "ADC VRef = %f ", (f_ai0000_Aux));
+ LCM_SetTextColor (Scale2RGBColor (0, 31, 0), Scale2RGBColor (31, 0, 31));
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 6), StringText);
+ f_ai0002_prec = f_ai0000_Aux;
+ }
+ }
+}
+
+/*
while (ui16_TestStep < 32) {
ui16_TestColor = Scale2RGBColor (ui16_R, ui16_G, ui16_B);
@@ -231,168 +284,4 @@
ui16_TestStep ++;
}
delay (2500);
-
-/*
- ui16_TestColor = 0x0000;
- ui16_TestStep = 0x0000;
- while (ui16_TestStep < 32) {
-
- LCM_ClearScreen (ui16_TestColor);
- LCM_SetTextColor(ui16_TestColor, (0xffff- ui16_TestColor));
- sprintf (StringText,
- "Color: %#04x %#04x ", ui16_TestColor, ui16_TestStep);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), StringText);
-
-// ui16_TestColor += 0x0841;
- ui16_TestColor += 0x0001;
- ui16_TestStep ++;
- }
-
- ui16_TestColor = 0x0000;
- ui16_TestStep = 0x0000;
- while (ui16_TestStep < 32) {
-
- LCM_ClearScreen (ui16_TestColor);
- LCM_SetTextColor(ui16_TestColor, (0xffff- ui16_TestColor));
- sprintf (StringText,
- "Color: %#04x %#04x ", ui16_TestColor, ui16_TestStep);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), StringText);
-
-// ui16_TestColor += 0x0841;
- ui16_TestColor += 0x0040;
- ui16_TestStep ++;
- }
-
- ui16_TestColor = 0x0000;
- ui16_TestStep = 0x0000;
- while (ui16_TestStep < 32) {
-
- LCM_ClearScreen (ui16_TestColor);
- LCM_SetTextColor(ui16_TestColor, (0xffff- ui16_TestColor));
- sprintf (StringText,
- "Color: %#04x %#04x ", ui16_TestColor, ui16_TestStep);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), StringText);
-
-// ui16_TestColor += 0x0841;
- ui16_TestColor += 0x0800;
- ui16_TestStep ++;
- }
-
- ui16_TestColor = 0x0000;
- ui16_TestStep = 0x0000;
- while (ui16_TestStep < 32) {
-
- LCM_ClearScreen (ui16_TestColor);
- LCM_SetTextColor(ui16_TestColor, (0xffff- ui16_TestColor));
- sprintf (StringText,
- "Color: %#04x %#04x ", ui16_TestColor, ui16_TestStep);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), StringText);
-
-// ui16_TestColor += 0x0841;
- ui16_TestColor += 0x0041;
- ui16_TestStep ++;
- }
-
- ui16_TestColor = 0x0000;
- ui16_TestStep = 0x0000;
- while (ui16_TestStep < 32) {
-
- LCM_ClearScreen (ui16_TestColor);
- LCM_SetTextColor(ui16_TestColor, (0xffff- ui16_TestColor));
- sprintf (StringText,
- "Color: %#04x %#04x ", ui16_TestColor, ui16_TestStep);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), StringText);
-
-// ui16_TestColor += 0x0841;
- ui16_TestColor += 0x0840;
- ui16_TestStep ++;
- }
-
- ui16_TestColor = 0x0000;
- ui16_TestStep = 0x0000;
- while (ui16_TestStep < 32) {
-
- LCM_ClearScreen (ui16_TestColor);
- LCM_SetTextColor(ui16_TestColor, (0xffff- ui16_TestColor));
- sprintf (StringText,
- "Color: %#04x %#04x ", ui16_TestColor, ui16_TestStep);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), StringText);
-
-// ui16_TestColor += 0x0841;
- ui16_TestColor += 0x0801;
- ui16_TestStep ++;
- }
-
- delay (5000);
-*/
-/*
- LCM_ClearScreen (0x07e0);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), "0x07e0");
- delay (1000);
- LCM_ClearScreen (0x07c0);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), "0x07c0");
- delay (1000);
- LCM_ClearScreen (0x07e0);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), "0x07e0");
- delay (1000);
- LCM_ClearScreen (0x07c0);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), "0x07c0");
- delay (1000);
-
- LCM_ClearScreen (0x0000);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), "0x0000");
- delay (1000);
- LCM_ClearScreen (0x0020);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), "0x0020");
- delay (1000);
- LCM_ClearScreen (0x0040);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), "0x0040");
- delay (1000);
- LCM_ClearScreen (0x0020);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), "0x0020");
- delay (1000);
- LCM_ClearScreen (0x0040);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), "0x0040");
- delay (1000);
- LCM_ClearScreen (0x0000);
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 4), "0x0000");
- delay (1000);
-
-*/
- ui16_TestColor = Scale2RGBColor (ui16_R, ui16_G, ui16_B);
- LCM_SetTextColor (
- Scale2RGBColor (0, 0, 0),
- Scale2RGBColor (31, 31, 31)
- );
- LCM_DrawString (0, 0+(TEXT_ROW_SPACING* 0), "You Start Me Up ...");
-
- while (1) {
- static int32_t Pulses_Prec;
- static uint32_t ms_0003_prec;
- int32_t i32_Pulses;
-
- i32_Pulses = Stabilus322699.getPulses();
-
- PosizionatoreSW (in_PosizionatoreSW, out_PosizionatoreSW);
- PWM_PB3.pulsewidth_us(((float)i32_Pulses/ (float)5000.0)* (float)100.0); // 0.. 100us -> 0.. 100%
-
- // LA: Wedge 4 LCDRefresh
- if (
- (i32_Pulses != Pulses_Prec)
- ) {
- sprintf (StringText,
- "Pulses: %d ", i32_Pulses);
- LCM_SetTextColor (Scale2RGBColor (0, 0, 31), Scale2RGBColor (31, 31, 0));
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 1), StringText);
- }
-
- if (out_PosizionatoreSW.ui32_PassedActual_ms != ms_0003_prec) {
- sprintf (StringText,
- "PassedActual_ms: %d ", out_PosizionatoreSW.ui32_PassedActual_ms);
- LCM_SetTextColor (Scale2RGBColor (0, 31, 0), Scale2RGBColor (31, 0, 31));
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 2), StringText);
- ms_0003_prec = out_PosizionatoreSW.ui32_PassedActual_ms;
- }
-// StabilusMenu (0, 0, out_PosizionatoreSW.ui32_PassedActual_ms, i32_Pulses);
- }
-}
+*/
\ No newline at end of file