Diff: main.cpp

Revision:

26:bf432a28d0c6

Parent:

25:3d3ebfd0a73f

--- a/main.cpp	Thu Dec 06 18:06:21 2018 +0000
+++ b/main.cpp	Mon May 13 08:42:21 2019 +0000
@@ -1,8 +1,8 @@
 /*
- *   Rotator program
+ *   CreaLASER program
  *
- *  Version 1.0, December 2, 2018
- *  for Nucleo32: L432
+ *  Version 1.0, Fevrier 11, 2019
+ *  for Nucleo32: F303
  *
  */
 
@@ -12,132 +12,350 @@
 Serial pc_uart(USBTX, USBRX);
 
 // ---------------- Local global variables --------------
-int UICommad=0;     // New Command from the UI
-int32_t CSpeed=0;       // Current Speed
-int32_t CMod=0;         // Current Mode: 0: Stop; 1: Run; -1: Zero
-int BTControl=0;    // Control taken by Bluetooth
-char ExecCommand;   // Command to be execute
+char        ExecCommand;   // Command to be execute
+float    AbsolutePosLaser = 0;
+float    AbsolutePosPrisme = 0;
+float    RelativePosLaser = 0;
+float    RelativePosPrisme = 0;
+float    ZeroPosLaser = 0;
+float    ZeroPosPrisme = 0;
+// Virtual target in the Upper Corner
+float    TargetPosLaser = 37;
+float    TargetPosPrisme = -14.0;
 // ---------------- PIN DEFINITIONS ---------------------
-DigitalOut myled(LED1);     // Blinking LED
-//InterruptIn  buttonBox(PB_1);
+DigitalOut  myled(LED1);     // Blinking LED
+// --- Define the Servos
+AsyncServo  LaserGun(PA_11);
+AsyncServo  Prisme(PA_8);
+DigitalIn   FireButton(PB_5, PullUp);
+// --- Define the Laser beam
+PwmOut  LaserLED(PA_12);
+#define DEFAULT_LASER   0.2
+DigitalIn PhotoDiode(PA_1);
+// ---------------- Musics --------------
+// Music song_greensleaves("Greensleaves:d=4,o=5,b=140:g,2a#,c6,d.6,8d#6,d6,2c6,a,f.,8g,a,2a#,g,g.,8f,g,2a,f,2d,g,2a#,c6,d.6,8e6,d6,2c6,a,f.,8g,a,a#.,8a,g,f#.,8e,f#,2g");
+// Music song_lightmyfire("LightMyFire:d=4,o=5,b=140:8b,16g,16a,8b,8d6,8c6,8b,8a,8g,8a,16f,16a,8c6,8f6,16d6,16c6,16a#,16g,8g#,8g,8g#,16g,16a,8b,8c#6,16b,16a,16g,16f,8e,8f,1a,a");
+// Music song_xfile("Xfiles:d=4,o=5,b=140:e,b,a,b,d6,2b.");
+// PIN for the Buzzer
+// Buzzer buzzer(PB_0);
+// Note la("A#4",50);  //the sound
+
+//--- SPI Multiplexed Display
+SPI Display(PA_7, NC, PA_5);    // 8 digit 7 Segment Serial Multiplexed DISPLAY using SPI
+//-------------------------------------------------------------
+
+// --- Define the display and the Messages
+Ticker  DisplayTimer;               // Display is managed by Timer IT
+volatile    int     DisplayDate[8] = {2, 0, 1, 9, 0, 2, 1, 2 }; // Default Date
+volatile    int     DisplayIndex = 0;   // Index for Display
+volatile    int     DisplayBlink = 0;   // Display Blinking
+volatile    int     DisplayDot5 = 1;    // Display Dot on time
+#define DISPLAY_US  500
+int     BlinkHalfPeriod_us = 200000/DISPLAY_US;  // Blinking Period is us
+int     BlinkIndex  = 0;
+
+// --- Clear Display 
+void ClearDisplay()
+{
+        Display.write(0x0000);
+}
+
+// --- Write one digit at one location
+void    DigitDisplay(uint32_t val, uint32_t dot, uint32_t digit)
+{
+   const   uint32_t SevenSeg[26] = {
+ // dp,   G,    F,    E,    D,    C,    B,    A
+ //0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01 (negative logic)
+ // -- Chiffres
+        0x40, // 0
+        0x79, // 1
+        0x24, // 2
+        0x30, // 3
+        0x19, // 4
+        0x12, // 5
+        0x02, // 6
+        0x78, // 7
+        0x00, // 8
+        0x18, // 9
+// -- Won
+        0x43,   //W1
+        0x61,   //W2
+        0x23,   //o
+        0x2B,   //n
+// -- LoST
+        0x47,   //L
+        0x23,   //o
+        0x12,   //S
+        0x07,   //T
+// -- CodE
+        0x46,   //C
+        0x23,   //o
+        0x21,   //d
+        0x06,   //E
+// -- Empty
+        0x7F,   // Empty
+        0x77,   // _
+        0x3F,   // -
+        0x0C    // P
+        };
+    const uint32_t  PosSeg[8] = {
+        0x0001, 0x0002, 0x0004, 0x0008, 0x0010, 0x0020, 0x0040, 0x0080,
+        };
+    if (dot) {
+        Display.write((SevenSeg[val]<<8)| PosSeg[digit]);
+        }
+    else    {
+        Display.write((SevenSeg[val]<<8)| PosSeg[digit] | 0x8000);
+        }
+}
 
-// --- Define the Four PINs & Time of movement used for Motor drive -----
-Motor motorRotator(PA_8, PA_11, PB_5, PB_4, 1000);
-// --- Define the LEDS
-int NoLED; // No LED
-DigitalOut LedRouge(PB_7);
-DigitalOut LedJaune(PB_0);
-DigitalOut LedVerte(PA_12);
-// --- Define the Speed Potentiometer
-AnalogIn    SpeedPlus(PA_7);
-AnalogIn    SpeedMoins(PA_6);
-// --- Define the three mode switch
-DigitalIn   ControlZero(PA_0, PullUp);
-DigitalIn   ControlStop(PA_1, PullUp);
-DigitalIn   ControlRun(PA_3, PullUp);
+void    DisplayITHandler()
+{
+    if (DisplayBlink) {
+        if (BlinkIndex < BlinkHalfPeriod_us) {
+            // Display date
+            DigitDisplay(DisplayDate[DisplayIndex], (DisplayIndex==5)&&DisplayDot5, DisplayIndex);
+            DisplayIndex = (DisplayIndex+1)%8;
+            }
+        else {
+            ClearDisplay();
+            }
+        // Increment Blink Index
+        BlinkIndex = (BlinkIndex++) % (2*BlinkHalfPeriod_us);
+        }
+    else { 
+        DigitDisplay(DisplayDate[DisplayIndex], (DisplayIndex==5)&&DisplayDot5, DisplayIndex);
+        DisplayIndex = (DisplayIndex+1)%8;
+        }
+}
+
+void    DisplayWon()
+{
+        DisplayDate[0] = 10;
+        DisplayDate[1] = 11;
+        DisplayDate[2] = 12;
+        DisplayDate[3] = 13;
+        DisplayBlink=1;
+        DisplayDate[4] = 22;
+        DisplayDate[5] = 22;
+        DisplayDate[6] = 22;
+        DisplayDate[7] = 22;
+        DisplayDot5=0;       
+}
+
+void    DisplayCode()
+{
+        DisplayDate[0] = 18;
+        DisplayDate[1] = 19;
+        DisplayDate[2] = 20;
+        DisplayDate[3] = 21;
+        DisplayBlink=0;
+        DisplayDate[4] = 2;
+        DisplayDate[5] = 23;
+        DisplayDate[6] = 23;
+        DisplayDate[7] = 23;       
+        DisplayDot5=0;       
+}
 
+void    DisplayLost()
+{
+        DisplayDate[0] = 14;
+        DisplayDate[1] = 15;
+        DisplayDate[2] = 16;
+        DisplayDate[3] = 17;
+        DisplayBlink=1;
+        DisplayDate[4] = 22;
+        DisplayDate[5] = 22;
+        DisplayDate[6] = 22;
+        DisplayDate[7] = 22;       
+        DisplayDot5=0;       
+}
+
+void    DisplayFire()
+{
+        DisplayDate[0] = 24;
+        DisplayDate[1] = 24;
+        DisplayDate[2] = 24;
+        DisplayDate[3] = 24;
+        DisplayBlink=1;
+        DisplayDate[4] = 24;
+        DisplayDate[5] = 24;
+        DisplayDate[6] = 24;
+        DisplayDate[7] = 24;       
+        DisplayDot5=0;       
+}
+
+void DisplayRelativePosition()
+{
+        int32_t ir;
+        // Display Prisme Angle
+        DisplayDate[0] = 25;    // P
+        ir = int(AbsolutePosPrisme - ZeroPosPrisme);
+        if (ir>=0)  DisplayDate[1] = 22; // Empty
+        else {
+            DisplayDate[1] = 24; // - sign
+            ir=-ir;
+            }
+        DisplayDate[2] = ir / 10;
+        DisplayDate[3] = ir % 10;
+        // Display Laser Angle
+        DisplayDate[4] = 14;    // L
+        ir = int(AbsolutePosLaser - ZeroPosLaser);
+        if (ir>=0)  DisplayDate[5] = 22; // Empty
+        else { 
+            DisplayDate[5] = 24; // - sign
+            ir=-ir;
+            }
+        DisplayDate[6] = ir / 10;
+        DisplayDate[7] = ir % 10;
+        DisplayDot5=0;       
+        DisplayBlink=0;
+}
 // ------------ Process ----------------------------------
-Ticker  HWUI;
-
 void help() // Display list of Commands
 {
     DEBUG("List of commands:\n\r");
     DEBUG(" h --> Help, display list of cammands\n\r");
 }
 
-/* Stop all processes */
-void stop_all()
-{
-    motorRotator.Stop();
-}
-
-int32_t    LireSpeed()
-{
-    double  inp;
-    double  inm;
-    int32_t TargetSpeed;
-    inp=(double) SpeedPlus.read();
-    inm=(double) SpeedMoins.read();
-    TargetSpeed=int((inp-inm)*15.0);
-    if (TargetSpeed > 9) TargetSpeed = 9;
-    if (TargetSpeed < -9) TargetSpeed = -9;
-    return(TargetSpeed);
-}
-
-int32_t    LireControl()
-{
-    if (ControlStop==0) return(0);
-    if (ControlRun==0) return(1);
-    if (ControlZero==0) return(-1);
-    return(0);
-}
-
-void    TestHW()
-{
-    LedRouge=1;
-    wait(0.5);
-    LedJaune=1;
-    wait(0.5);
-    LedVerte=1;
-    wait(0.5);
-    LedRouge=0;
-    wait(0.5);
-    LedJaune=0;
-    wait(0.5);
-    LedVerte=0;
-    wait(0.5);
-}
-
-void    HardwareUI()
-{
-    // Display Motor status with Green  and Yellow LEDs
-    if (!NoLED) {
-        if (motorRotator.GetState() == Motor_RUN) LedVerte=1;
-        else if (motorRotator.GetState() == Motor_ZERO) LedVerte = !LedVerte;
-        else LedVerte=0;
-        if (motorRotator.direction==CLOCKWISE) LedJaune=1;
-        else LedJaune=0;
-        LedRouge=BTControl;
-    }
-}
-
-char    ComposeCommandFromUI()
-{
-    char NewUICommand='-';   // Null command
-    if (!BTControl) {
-        int32_t TSp;
-        int32_t TMod;
-        int32_t TSpeed;
-        int32_t TDir;
-        TSp=LireSpeed();
-        if (TSp>0) TDir=CLOCKWISE;
-        if (TSp<0) TDir=COUNTERCLOCKWISE;
-        TSpeed=abs(TSp);
-        TMod=LireControl();
-        if (CMod!=TMod) {
-            if (TMod==0) NewUICommand = 'S';
-            if (TMod==1) {
-                if (TDir == CLOCKWISE) NewUICommand='R';
-                else NewUICommand='L';
-            }
-            if (TMod==-1) NewUICommand = 'Z';
-            CMod=TMod;
-        } else {
-            if (TSpeed!=CSpeed) {
-                if (TSpeed==0)NewUICommand = 'S';
-                else NewUICommand = char('0'+TSpeed);
-                CSpeed=TSpeed;
-            }
-        }
-    }
-    return (NewUICommand);
-}
-
 /* Message */
 void message()
 {
-    DEBUG("-------------------------------------------------------\n\r");
-    DEBUG("-------------------------------------------------------\n\r");
+    DEBUG("-----------------------------------------------------\n\r");
+    DEBUG("-----        CreaLASER        version 1.0       -----\n\r");
+    DEBUG("-----     faite par fbd38 le 11 Fevrier 2019    -----\n\r");
+    DEBUG("-----------------------------------------------------\n\r");
+}
+
+void Fire()
+{
+    float epsi = 2.0;
+    // Make some noise
+    // PLAY_MUSIC(song_xfile,buzzer);
+    DisplayFire();
+    LaserLED=1.0;
+    wait(2);
+    if (((abs(TargetPosLaser - AbsolutePosLaser) < epsi) 
+        && (abs(TargetPosPrisme - AbsolutePosPrisme) < epsi))
+        || (PhotoDiode)) {
+    // --- WINNER
+     DisplayWon();
+     // PLAY_MUSIC(song_lightmyfire,buzzer);
+     wait(2);
+     DisplayCode();
+     wait(10);
+     }
+    else  {
+        DisplayLost();
+        // PLAY_MUSIC(song_greensleaves,buzzer);
+        }
+    wait(3);
+    LaserLED=DEFAULT_LASER;
+    // Set back all servos to zero
+    AbsolutePosPrisme = ZeroPosPrisme;
+    AbsolutePosLaser = ZeroPosLaser;
+    Prisme.position(AbsolutePosPrisme);
+    wait(1);
+    LaserGun.position(AbsolutePosLaser);
+}
+
+void    StoreZeroPosition()
+{
+    ZeroPosLaser = AbsolutePosLaser;
+    ZeroPosPrisme = AbsolutePosPrisme;
+}
+
+void    StoreTargetPosition()
+{
+    TargetPosLaser = AbsolutePosLaser;
+    TargetPosPrisme = AbsolutePosPrisme;
+}
+
+void ReturnAbsolutePosition()
+{
+    char bt_text[128];
+    sprintf(bt_text, "*L%4.0f**P%4.0f*", AbsolutePosLaser, AbsolutePosPrisme);
+    DEBUG(bt_text);
+}
+
+void RotateRelative()
+{   
+    // Parse the servo
+    char c;
+    char AngleStr[4]="";
+    float Angle;
+    float Amax=90.0;
+    float Amin=-90.0;
+    int i,j;
+    int servo_type = 0; // 1=Laser, 2=Prisme
+    while (! bt_uart.readable()) /* Wait */;
+    c=bt_uart.getc();
+    pc_uart.printf("%c", c);
+    if ((c == 'a') || (c == 'A')) servo_type=1; // Laser
+    if ((c == 'b') || (c == 'B')) servo_type=2; // Prisme
+    for (i=j=0; i<4; i++) {
+        while (! bt_uart.readable()) /* Wait */;
+        c=bt_uart.getc();
+        pc_uart.printf("%c", c);
+        if ((c != ' ') && (c != ',')) {
+            AngleStr[j++] = c;
+            AngleStr[j] = 0;
+            }
+        if ( c == ',') i=4;
+        }
+    pc_uart.printf(" Angle String %s", AngleStr);
+    sscanf(AngleStr, "%f", &Angle);
+    pc_uart.printf(" Angle Float %4.1f", Angle);
+    if (Angle >  Amax) Angle =  Amax;
+    if (Angle < Amin) Angle = Amin;
+    if (servo_type == 1) {
+        RelativePosLaser = Angle;
+        AbsolutePosLaser = RelativePosLaser + ZeroPosLaser;
+        if (AbsolutePosLaser >  Amax) AbsolutePosLaser =  Amax;
+        if (AbsolutePosLaser < Amin) AbsolutePosLaser = Amin;
+        LaserGun.position(AbsolutePosLaser);
+        }
+    if (servo_type == 2) {
+        RelativePosPrisme = Angle;
+        AbsolutePosPrisme = RelativePosPrisme + ZeroPosPrisme;
+        if (AbsolutePosPrisme >  Amax) AbsolutePosPrisme =  Amax;
+        if (AbsolutePosPrisme < Amin) AbsolutePosPrisme = Amin;
+        Prisme.position(AbsolutePosPrisme);
+        }
+    wait(0.1);
+}
+
+void RotateIncremetal()
+{   
+    // Parse the servo
+    char c;
+    float Amax=90.0;
+    float Amin=-90.0;
+    float Angle=1.0;    // Increment for manual adjustement
+    while (! bt_uart.readable()) /* Wait */;
+    c=bt_uart.getc();
+//    pc_uart.printf("%c", c);
+    if (c=='a') {
+        AbsolutePosLaser += Angle;
+        if (AbsolutePosLaser >  Amax) AbsolutePosLaser =  Amax;
+        LaserGun.position(AbsolutePosLaser);
+        }
+    if (c=='b') {
+        AbsolutePosPrisme += Angle;
+        if (AbsolutePosPrisme >  Amax) AbsolutePosPrisme =  Amax;
+        Prisme.position(AbsolutePosPrisme);
+        }
+    if (c=='z') {
+        AbsolutePosLaser -= Angle;
+        if (AbsolutePosLaser < Amin) AbsolutePosLaser = Amin;
+        LaserGun.position(AbsolutePosLaser);
+        }
+    if (c=='n') {
+        AbsolutePosPrisme -= Angle;
+        if (AbsolutePosPrisme < Amin) AbsolutePosPrisme = Amin;
+        Prisme.position(AbsolutePosPrisme);
+        }
+    wait(0.1);
 }
 
 /* Main Routine */
@@ -146,43 +364,41 @@
     myled = 1;      // To see something is alive
     bool flaghelp;
 // -- Uncomment to program the HC-06 Module
-//    DEBUG("AT+NAMECreaRoll_x");
+//    DEBUG("AT+NAMECreaLASER01");
 //    wait(10);
-    if (ControlZero==0) NoLED=1;
-    else {
-        NoLED=0;
-        TestHW();
-    }
-    DEBUG("-------------------------------------------------------\n\r");
-    DEBUG("-----        CreaRoll         version 1.0         -----\n\r");
-    DEBUG("-----     faite par fbd38 le   2 Dec 2018         -----\n\r");
-    DEBUG("-------------------------------------------------------\n\r");
+    DEBUG("-----------------------------------------------------\n\r");
+    DEBUG("-----        CreaLASER        version 1.0       -----\n\r");
+    DEBUG("-----     faite par fbd38 le 11 Fevrier 2019    -----\n\r");
+    DEBUG("-----------------------------------------------------\n\r");
 //    DEBUG("SystemCoreClock = %d Hz =\n\r", SystemCoreClock);
 //    DEBUG("Wait 2s\n\r");
     myled = 0;      // Real stuff starts here
-    motorRotator.setSpeed(20.0); /* Default */
-    // Install UI
-    HWUI.attach(&HardwareUI, 0.3);
-
+    // Initializethe The Display SPI
+    Display.format(16,0);
+    // Display.frequency(50000);
+    ClearDisplay();
+    // Define the Display IT Task every 10ms
+    DisplayTimer.attach_us(&DisplayITHandler, DISPLAY_US); // Attache the Motor routine to the timer 
+    // Play a single tone 
+    // PLAY_NOTE(la, buzzer);
+    // Set all servos to zero
+    wait(0.1);
+    Prisme.position(AbsolutePosPrisme);
+    wait(1);
+    LaserGun.position(AbsolutePosLaser);
+    // Set default Laser Power
+    LaserLED=DEFAULT_LASER;
+    //
     while(1) {
         ExecCommand='-';    // Clear The Command List
         /*      Wait Until some command is in the pipe
-         *          eitheir from BT module or HWUI
          */
         if (bt_uart.readable()) {
-            char BTlc;   // BT Character to execute
-            BTlc = bt_uart.getc();
-            if (BTlc=='!') {
-                BTControl = 1;
-            } else {
-                if (BTlc=='.') {
-                    BTControl=0;
-                } else if (BTControl) ExecCommand=BTlc;
-            }
+            ExecCommand=bt_uart.getc();
         }
-        // In cas of not controlled by BT, get command from the HW UI
-        if (!BTControl) {
-            ExecCommand=ComposeCommandFromUI();
+        else {
+            // Check UI
+            if (FireButton == 0) ExecCommand = 'F';
             }
         // ------ Test according with the receved command
         if (ExecCommand!='-') {
@@ -192,28 +408,35 @@
                 case 'h':
                     help();
                     flaghelp=true;
-                    CASE('R', "Infinite Clockwise", motorRotator.RunInfinite(CLOCKWISE);    )
-                    CASE('L', "Infinite Counterclockwise", motorRotator.RunInfinite(COUNTERCLOCKWISE);    )
-                    CASE('Z', "Go Back to Zero", motorRotator.SetZero(); )
-                    CASE('z', "Define the Zero", motorRotator.DefineZero(); )
-                    CASE('r', "15deg Clockwise", motorRotator.RunDegrees(CLOCKWISE, (float)15.0);       )
-                    CASE('l', "15deg Counterclockwise", motorRotator.RunDegrees(COUNTERCLOCKWISE, (float)15.0);  )
-                    CASE('1', "Speed 10m/t", motorRotator.setSpeed(600.0);  )
-                    CASE('2', "Speed 2m/t", motorRotator.setSpeed(120.0);  )
-                    CASE('3', "Speed 1m/t", motorRotator.setSpeed(60.0);  )
-                    CASE('4', "Speed 30s/t", motorRotator.setSpeed(30.0);  )
-                    CASE('5', "Speed 20s/t", motorRotator.setSpeed(20.0);  )
-                    CASE('6', "Speed 15s/t", motorRotator.setSpeed(15.0);  )
-                    CASE('7', "Speed 10s/t",motorRotator.setSpeed(10.0);    )
-                    CASE('8', "Speed 7s/t",motorRotator.setSpeed(7.0);    )
-                    CASE('9', "Speed 5s/t",motorRotator.setSpeed(5.0);    )
-                    CASE('0', "STOP ", stop_all();   )
-                    CASE('S', "STOP ", stop_all();   )
+                    CASE('P', "Rotate Relative 'P[a/b]angle,' (Angle signed integer -90, 90)", RotateRelative(); )
+                    CASE('F', "  FIRE  ", Fire(); )
+                    // - commans used by Degug and Calibration
+                    CASE('-', "------------- Degug Only Commands --------------", message(); )
+                    CASE('L', "Laser ON", LaserLED=1.0; )
+                    CASE('l', "Laser low power", LaserLED=DEFAULT_LASER; )
+                    CASE('z', "Laser OFF", LaserLED=0.0; )
+                    CASE('r', "Rotate Incremental 'r[a/z/b/n]' (a/z for laser +,-; b/n for prisme +/-)", RotateIncremetal(); )
+                    CASE('S', "Store Zero Position", StoreZeroPosition(); )
+                    CASE('T', "Store Target Position", StoreTargetPosition(); )
+                    CASE('!', "Return Absolute Position", ReturnAbsolutePosition(); )
+                    CASE('W', "Display Won", DisplayWon(); )
+                    CASE('w', "Display Lost", DisplayLost(); )
+                    CASE('C', "Display Code", DisplayCode(); )
                     CASE('?', "Mistery", message(); )
 
                 default :
                     DEBUG("invalid command; use: 'h' for help()");
             }
+            DisplayRelativePosition();
         }
+        // Check Photodiode 
+        if (PhotoDiode) {
+            myled=1;
+            DisplayDot5=1;
+            }
+        else {
+            myled=0;
+            DisplayDot5=0;
+            }
     }
 }
\ No newline at end of file