Diff: main.cpp

Revision:

12:8464be95bf76

Parent:

11:80b6c5d77073

diff -r 80b6c5d77073 -r 8464be95bf76 main.cpp
--- a/main.cpp	Mon Oct 02 19:50:39 2017 +0000
+++ b/main.cpp	Tue Jan 30 06:55:27 2018 +0000
@@ -4,7 +4,12 @@
 // 2016/04/01, Copyright (c) 2016 MIKAMI, Naoki
 //------------------------------------------------------------
 
+
+//For Flag if we need sample or not
 //#define ANGLE_ENABLE
+#define SERIAL_ENABLE
+//#define LED_ENABLE
+
 #include "ACM1602NI.hpp"
 #include "DRV8830.h"
 #include "IQS62x.h"
@@ -18,18 +23,33 @@
 #define SWITCH_PERIOD 50  //Cycle time[*50ms]
 #define TOTAL_TIMES 30000   //total times n
 #define TIMER_COUNT 0.01
-#define LOOP_WAITMS 50
+#define MIN_ANGLE -90
+
 
-#define DEGREE_SHIFT    100
-#define UP_THRESHOLD    10
-#define DOWN_THRESHOLD  30
+#define LOOP_WAITMS 50
+#define SPEED_RATIO 0.5
 
-#define L_UP  .95
-#define R_UP  .95
-#define L_DOWN  0.85
-#define R_DOWN  0.85
+#define UP_THRESHOLD    80
+#define DOWN_THRESHOLD  70
 
 
+#define NUM_LED 3
+#define MAX_LED_ANGLE 100
+#define MAX_LED_BRI 0.7
+#define HSV_SHIFT 0.0
+
+/*
+#define L_UP  .5
+#define R_UP  .5
+#define L_DOWN  .5
+#define R_DOWN  .5
+*/
+
+
+#define L_UP  0.7
+#define R_UP  0.7
+#define L_DOWN  0.65
+#define R_DOWN  0.65
 
 
 
@@ -48,16 +68,23 @@
 MotorMove mvalL;
 MotorMove mvalR;
 
+#ifdef SERIAL_ENABLE
+Serial pc(USBTX, USBRX); // tx, rx
+#endif
+
 #ifdef ANGLE_ENABLE
 IQS62xIO iqs62x;          // class for basic IQS62x block read and write
 #endif
 DigitalIn button1(USER_BUTTON);
 
 
+#ifdef LED_ENABLE
+PwmOut  LEDPWM[NUM_LED]= {D3,D6,D9};
+#endif
+
 DigitalIn in_switchs[]=
    {   DigitalIn(A0), DigitalIn(A1), DigitalIn(A2), DigitalIn(A3)};
 
-
 static int shaft_deg=0;
 static int shaft_speed=0;  
 static int g_timer=0;              //gloabl timer
@@ -66,6 +93,9 @@
 static int sp_index=0; //Movement mode 0-3
 
 static int initial_deg=0;
+static int degree_shift=0;
+
+float pwmPeriod=0.000005;
 
 DRV8830 motorL(i2c, DRV8830ADDR_NN);   //Motor1
 DRV8830 motorR(i2c, DRV8830ADDR_0N);   //Motor2
@@ -80,6 +110,12 @@
 int  MainIOloop();
 void DisplayStatus();
 void MoveMotor();
+void ShowLED(int angle);
+void HSV2RGB(const float *HSV, float *RGB);
+
+
+
+
 
 int main()
 {
@@ -93,25 +129,40 @@
     //Initialize Ic2 Device
 
 
-    motorL.speed(0);
-    motorR.speed(0);
+    //motorL.speed(0);
+    //motorR.speed(0);
     
     
     
     #ifdef ANGLE_ENABLE
-    lcd_.WriteStringXY("IQS_Init_Start",0,0);
-    iqs62x.configure(); // configure
+    lcd_.WriteStringXY("IQS_Calibration",0,0);
+    //wait(1);
+    //iqs62x.configure(); // configure
+    //wait(1);
+    lcd_.WriteStringXY("IQS_Cconfig done",0,0);
+    
+    iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers      
+    initial_deg = ReadDegree(iqs62x.registers);
+    
+    //degree_shift = (360- ReadDegree(iqs62x.registers) );
+
+
+    #ifdef LED_ENABLE
+    for (int i=0; i<NUM_LED ;i=i+1)
+    {
+        LEDPWM[i].period(pwmPeriod);
+        LEDPWM[i]=1;
+    }
+    #endif
+    
     wait(1);
-    iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers           
-    initial_deg = ReadDegree(iqs62x.registers);
     lcd_.WriteStringXY("IQS_Init_done ",0,0);
-    wait(1);
     #else
     lcd_.WriteStringXY("No_Sensor",0,0);
     initial_deg=0;
     #endif
     
-    
+
 
     
     
@@ -136,6 +187,18 @@
                 //motorR.speed( (shaft_deg-180.0)/200.0 );
                 wait_ms(LOOP_WAITMS);
                 MoveMotor();
+                
+                
+                #ifdef SERIAL_ENABLE
+                pc.printf("%d\r\n", shaft_deg);
+                #endif
+                
+                
+                /*
+                LEDPWM[0]=1;
+                LEDPWM[1]=1;
+                LEDPWM[2]=1;
+                */
                 cnt ++;
                 }    
 }
@@ -151,23 +214,44 @@
 
 //detect up or donw by thredold
          #ifdef ANGLE_ENABLE
-         if (shaft_deg> (UP_THRESHOLD+DEGREE_SHIFT) )
-          { 
-             bflag_up_cur=1;
-           }
-        else
-          {
-            bflag_up_cur=0;
-           };
+         
+         
+         
+         if( button1==1)
+         {   
+            if (shaft_deg>      (UP_THRESHOLD+degree_shift))
+                { 
+                  bflag_up_cur=1;
+                }
+            else
+                {
+                  bflag_up_cur=0;
+                };
+           
          
-         if (shaft_deg< (DOWN_THRESHOLD+DEGREE_SHIFT) )
-          { 
-             bflag_down_cur=1;
-           }
-        else
+             if (shaft_deg< (DOWN_THRESHOLD+degree_shift) )
+               { 
+                 bflag_down_cur=1;
+               }
+              else
+               {
+                 bflag_down_cur=0;
+               }
+               
+          }
+               
+               
+               
+        else if (button1==0)
           {
-            bflag_down_cur=0;
-           };
+               bflag_up_cur=0;
+               bflag_down_cur=1;                          
+          }
+               
+               
+           
+           
+           
            #else 
            if (button1==1)
             {
@@ -181,10 +265,26 @@
              } 
            #endif
            
+           
+           int mot_speed;
+           
+           if (shaft_speed==0)
+           {
+               mot_speed=1;
+            }
+            else
+            {
+               mot_speed = shaft_speed;
+            }
+               
+               
+           
 //send down or up command when status had changed
          if(bflag_up_pre==0&& bflag_up_cur==1)
               {
                 //shaft_speed
+//                mvalL.up_motor_set(cnt, L_UP * mot_speed * SPEED_RATIO);
+//                mvalR.up_motor_set(cnt, R_UP * mot_speed * SPEED_RATIO);
                 mvalL.up_motor_set(cnt, L_UP);
                 mvalR.up_motor_set(cnt, R_UP);
                 lcd_.WriteStringXY("U",0,1);
@@ -192,6 +292,8 @@
               
          else if(bflag_down_pre==0 && bflag_down_cur==1)
               {
+//                mvalL.down_motor_set(cnt, L_DOWN * mot_speed *SPEED_RATIO);
+//                mvalR.down_motor_set(cnt, R_DOWN * mot_speed *SPEED_RATIO );
                 mvalL.down_motor_set(cnt, L_DOWN);
                 mvalR.down_motor_set(cnt, R_DOWN);
                 lcd_.WriteStringXY("D",1,1);
@@ -210,14 +312,58 @@
         //motorL.speed(0.5);
         //motorR.speed(0.3);
         
-        lcd_.WriteValueXY("%1.2f",lspeed,3,1);
-        lcd_.WriteValueXY("%1.2f",rspeed,8,1);
+        lcd_.WriteValueXY("%1.2f ",lspeed*5,3,1);
+        lcd_.WriteValueXY("%1.2f ",rspeed*5,8,1);
         
          
          bflag_up_pre   = bflag_up_cur;
          bflag_down_pre = bflag_down_cur;
     }
     
+    
+    
+#ifdef LED_ENABLE
+void ShowLED(int angle)
+    {
+        float led_H;
+        float led_V;
+        
+        led_H = (float)angle/(float)MAX_LED_ANGLE+HSV_SHIFT;
+        //led_val = (float)angle/(float)MAX_LED_ANGLE;
+        led_V = (float)angle/(float)MAX_LED_ANGLE;
+        
+
+        if (led_H>1.0)
+        {
+           led_H = led_H - 1.0;
+        }
+        
+        
+        
+        float RGB[3];
+        float HSV[3];
+        
+        
+        
+        HSV[0]=led_H;
+        HSV[1]=1;
+        HSV[2]=MAX_LED_BRI;
+        
+        
+        HSV2RGB(HSV,RGB);
+
+
+        LEDPWM[0]= 1.0-RGB[0]*1.1;
+        LEDPWM[1]= 1.0-RGB[1];
+        LEDPWM[2]= 1.0-RGB[2];
+        
+        
+        
+    }
+    
+#endif
+        
+        
 
 
 void DisplayStatus()
@@ -235,18 +381,29 @@
 {
               static int cnt=0;
               #ifdef ANGLE_ENABLE
-              iqs62x.waitForIqsReady();
+              //iqs62x.waitForIqsReady();
               iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers           
-              shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+ DEGREE_SHIFT;
-              #endif
+              //shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+ DEGREE_SHIFT;
+                          
+               
+              shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+degree_shift;
               
-              
+              if (shaft_deg<MIN_ANGLE)
+               {
+                  shaft_deg=shaft_deg+360;
+               }
+              else if(shaft_deg>(MIN_ANGLE+360))
+               {
+                shaft_deg= shaft_deg-360;
+                }
+              /*
               if(shaft_deg<0)
               {
                 shaft_deg = shaft_deg+360; // offset 100deg to cancel error 
                }
-               
-               
+               */               
+#endif
+                             
               #ifdef ANGLE_ENABLE
               shaft_speed= ReadSpeed(iqs62x.registers);
               //lcd_.WriteValueXY("%3d ",k, 0,0);
@@ -264,6 +421,10 @@
               }
               */
               
+              
+              #ifdef LED_ENABLE
+              ShowLED(shaft_deg);
+              #endif
 
 
               cnt++;
@@ -348,8 +509,57 @@
        { 
         lcd_.WriteString(" ");          
         }
-    }        
+    }      
+    
+}  
+    
+void HSV2RGB(const float *HSV, float *RGB) {
+    float h = HSV[0];
+    float s = HSV[1];
+    float v = HSV[2];
+    
+    float r = v;
+    float g = v;
+    float b = v;
+    
+    if (s > 0.0f) {
+        h *= 6.0f;
+        int i = (int) h;
+        float f = h - (float) i;
+        switch (i) {
+            default:
+            case 0:
+                g *= 1 - s * (1 - f);
+                b *= 1 - s;
+                break;
+            case 1:
+                r *= 1 - s * f;
+                b *= 1 - s;
+                break;
+            case 2:
+                r *= 1 - s;
+                b *= 1 - s * (1 - f);
+                break;
+            case 3:
+                r *= 1 - s;
+                g *= 1 - s * f;
+                break;
+            case 4:
+                r *= 1 - s * (1 - f);
+                g *= 1 - s;
+                break;
+            case 5:
+                g *= 1 - s;
+                b *= 1 - s * f;
+                break;
+        }
+    }
+        
+    RGB[0] = r;
+    RGB[1] = g;
+    RGB[2] = b;
 }
+    
+    
+    
 
-
-