Revision 8:f7ad1d7176ba, committed 2017-09-30

Comitter:

8mona

Date:

Sat Sep 30 16:43:51 2017 +0000

Parent:

7:acb1074eaba6

Child:

9:b58e7d72a91c

Commit message:

Activate IQS62X and motor and IO Switch

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorMove.cpp	Sat Sep 30 16:43:51 2017 +0000
@@ -0,0 +1,130 @@
+#include    "mbed.h"
+#include    "DRV8830.h"
+#include    "MotorMove.h"
+#define SPEED_RATIO  1.0
+
+//float vol_decel_ini[DECEL_SIZE] = {1,0.75,0.4,0.15,0.05,0};
+float vol_decel_ini[DECEL_SIZE] = {-0.5,0,0,0,0,0};
+float vol_accel_ini[ACCEL_SIZE] = {0,0.5,0.8,1,1,1};
+
+void MotorMove::up_motor_set(int time_counter, int speed)
+    {
+        bflag_up =1;
+        bflag_down =0;
+        start_time_count=time_counter;
+
+        for (int i=0;i<ACCEL_SIZE;i++)
+        {
+             vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
+        }                
+                
+        for (int i=0;i<DECEL_SIZE;i++)
+        {
+             vol_decel[i] = vol_decel_ini[i]*speed/SPEED_RATIO;
+        }
+    };
+
+        
+void MotorMove::down_motor_set(int time_counter, int speed)
+    {
+        bflag_up =0;
+        bflag_down=1;
+        start_time_count=time_counter;
+                
+        for (int i=0;i<ACCEL_SIZE;i++)
+        {
+             vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
+        }  
+        
+        for (int i=0;i<DECEL_SIZE;i++)
+        {
+             vol_decel[i] = vol_decel_ini[i]*speed/SPEED_RATIO;
+        }
+    };
+    
+    
+float MotorMove::ReturnMotorVol(int time_counter, int sw_flag1,int sw_flag2)
+    {
+        float speed;
+        int time = time_counter - start_time_count;
+        
+        
+        static int sw_flag1_pre=0;
+        static int sw_flag2_pre=0;
+        static int stop_time=0;
+        
+        static int up_decel_count=0;
+        static int down_decel_count=0;
+        
+        //decel timer check
+        if(bflag_up==1)
+           up_decel_count==0;
+        if(bflag_down==1)
+           down_decel_count==0;
+           
+        
+        
+        if(bflag_up)
+         {
+             if (sw_flag2==1 && sw_flag2_pre==0) //State changed 
+             {
+                stop_time=time_counter;
+             }  
+
+            if(sw_flag2==0) // move motor
+             {
+                if(time>ACCEL_SIZE)
+                   {
+                     time = ACCEL_SIZE;
+                   }
+                speed = vol_accel[time];
+             }
+             
+            else if  (sw_flag2==1)
+             { 
+              time = time_counter-stop_time;
+              
+                if(time>DECEL_SIZE)
+                   {
+                     time = DECEL_SIZE;
+                     bflag_up =0;
+                   }
+                speed = vol_decel[time];
+            }
+         }
+         
+        else if (bflag_down)
+         {
+             if (sw_flag1==1 && sw_flag1_pre==0) //State changed 
+             {
+                stop_time=time_counter;
+             }  
+
+            if(sw_flag1==0) // move motor
+             {
+                if(time>ACCEL_SIZE)
+                   {
+                     time = ACCEL_SIZE;
+                   }
+                speed = -vol_accel[time];
+             }
+             
+            else if  (sw_flag1==1)
+             { 
+              time = time_counter-stop_time;
+                if(time>DECEL_SIZE)
+                   {
+                     time = DECEL_SIZE;
+                     bflag_down =0;
+                   }
+                speed = -vol_decel[time];
+             }
+         }
+         
+        sw_flag1_pre=sw_flag1;
+        sw_flag2_pre=sw_flag2;        
+        return speed;       
+    };
+    
+  
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorMove.h	Sat Sep 30 16:43:51 2017 +0000
@@ -0,0 +1,31 @@
+#include    "mbed.h"
+//#include    "DRV8830.h"
+
+
+#define ACCEL_SIZE 6
+#define DECEL_SIZE 6
+#define LIMIT_TIME 30
+
+class MotorMove{
+    
+    public:
+     //DRV8830 motor; //Motor
+     float vol_decel[DECEL_SIZE];
+     float vol_accel[ACCEL_SIZE];
+     float down_ratio;
+     
+     
+     
+    void up_motor_set  (int time_counter, int speed);
+    void down_motor_set (int time_counter, int speed);
+    float ReturnMotorVol(int time_counter, int sw_flag1,int sw_flag2);
+    
+    private:
+      int start_time_count;
+      float Voltage;
+      
+      int bflag_up;
+      int bflag_down;
+      };
+      
+    
\ No newline at end of file

--- a/main.cpp	Fri Sep 22 16:27:18 2017 +0000
+++ b/main.cpp	Sat Sep 30 16:43:51 2017 +0000
@@ -8,10 +8,7 @@
 #include "DRV8830.h"
 #include "IQS62x.h"
 #include "IQSdisplayTerminal.h"
-
-
-//IQS62xDisplay terminal;   // class to display IQS62x registers on a terminal
-IQS62xIO iqs62x;          // class for basic IQS62x block read and write
+#include "MotorMove.h"
 
 //Cycle 
 #define UP_DURATION    14  //On time   [*100ms]
@@ -19,27 +16,17 @@
 #define DOWN_DURATION   8  //Down time [*50ms]
 #define SWITCH_PERIOD 50  //Cycle time[*50ms]
 #define TOTAL_TIMES 30000   //total times n
-#define ACCEL_SIZE 6
-#define DECEL_SIZE 6
+#define TIMER_COUNT 0.01
+#define LOOP_WAITMS 30
 
+#define UP_THRESHOLD    10
+#define DOWN_THRESHOLD  90
 
 
-const float spd_table[] = {1.0,0.9,0.8};
-//const float vol_accel[ACCEL_SIZE] = {0,0.3,0.6,0.8,0.95,1};
-const float vol_decel[DECEL_SIZE] = {1,0.75,0.4,0.15,0.05,0};
-const float vol_accel[ACCEL_SIZE] = {0,0.5,0.8,1,1,1};
-//const float vol_decel[DECEL_SIZE] = {1,0.75,0.15,-1,-0.2,0};
-
-const float donw_ratio=0.85;
-
-static int sp_index=0;
-
+Ticker timer_;
 
 using namespace Mikami;
-void ShowLCD(char * buffer,  int startbyte, int endbyte); // for wheel output
-
 Acm1602Ni lcd_;                               // Default, OK
-
 //Acm1602Ni lcd_(D14, D15);                     // OK
 //Acm1602Ni lcd_(D14, D15, 200000);             // OK
 //Acm1602Ni lcd_(D14, D15, 200000, true, true); // OK
@@ -47,27 +34,233 @@
 //Acm1602Ni lcd_(PC_9, PA_8);                   // OK
 //Acm1602Ni lcd_(PB_4, PA_8);                   // OK 
 
-Ticker timer_;
 I2C     i2c(D14, D15);
-DRV8830 motor(i2c, DRV8830ADDR_NN);
+DRV8830 motorL(i2c, DRV8830ADDR_NN);   //Motor1
+DRV8830 motorR(i2c, DRV8830ADDR_0N);   //Motor2
+MotorMove mvalL;
+MotorMove mvalR;
+
+IQS62xIO iqs62x;          // class for basic IQS62x block read and write
 InterruptIn button1(USER_BUTTON);
-static float motor_speed;
+
+
+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
+static int cnt;   //total timer of loop 
+static int sw_in[4]={0,0,0,0};     //swithc flag bit
+static int sp_index=0; //Movement mode 0-3
+
+static int initial_deg=0;
+
+
+void ShowLCD(char * buffer,  int startbyte, int endbyte); // for wheel output
+int ReadDegree(char * buffer);
+int ReadSpeed(char * buffer);
+void Displaylevel (int deg);
+void TimerIsr();
+void flip();
+int  MainIOloop();
+void DisplayStatus();
+void MoveMotor();
+
+int main()
+{
+   //initialize system    
+   static int time_prev;
+   for (int i=0;i<4;i=i++){
+       in_switchs[i].mode(PullUp);
+       }
+              
+    //motor.speed(0);
+    //Initialize Ic2 Device
+    motorL.speed(0);
+    motorR.speed(0);
+    
+    lcd_.WriteStringXY("IQS_Init_Start",0,0);
+    iqs62x.configure(); // configure
+    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);
+    
+    //read 0deg for initialize
+    button1.fall(&flip);
+
+//TimerIsr();
+//timer_.attach(&TimerIsr, TIMER_COUNT);
+//    bool status = motor.status();
+//   if (status & DRV8830_F_FAULT){
+//        motor.reset();
+//     } 
+   
+//Read here as Asynchronous when data gets ready
+while (true) {             
+                int time_current = g_timer;
+                int time_diff = time_current - time_prev;
+                int a=  MainIOloop();     
+                DisplayStatus();                
+                //display_info
+                time_prev = time_current;
+                //motorR.speed( (shaft_deg-180.0)/200.0 );
+                wait_ms(LOOP_WAITMS);
+                MoveMotor();
+                                
+                cnt ++;
+                }
+            
+            
+}
+
+void MoveMotor(){
+         static int bflag_up_pre=0;
+         static int bflag_down_pre=0;
+         int bflag_up_cur  =0;
+         int bflag_down_cur =0;
+         float lspeed;
+         float rspeed;
+
 
- 
-// Display elapsed time in minutes and seconds
+//detect up or donw by thredold        
+         if (shaft_deg>UP_THRESHOLD)
+          { 
+             bflag_up_cur=1;
+           }
+        else
+          {
+            bflag_up_cur=0;
+           };
+         
+         if (shaft_deg<DOWN_THRESHOLD)
+          { 
+             bflag_down_cur=1;
+           }
+        else
+          {
+            bflag_down_cur=0;
+           };
+           
+           
+//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, 1.0);
+                mvalR.up_motor_set(cnt, 1.0);
+                lcd_.WriteStringXY("U",0,1);
+              }
+         else if(bflag_down_pre==0 && bflag_down_cur==1)
+              {
+                mvalL.down_motor_set(cnt, 1.0);
+                mvalR.down_motor_set(cnt, 1.0);
+                lcd_.WriteStringXY("D",1,1);
+              }
+              
+        else{
+               lcd_.WriteStringXY("__",0,1);
+            }
+        
+        
+        lspeed=mvalL.ReturnMotorVol(cnt, sw_in[0],sw_in[1]);
+        rspeed=mvalR.ReturnMotorVol(cnt, sw_in[2],sw_in[3]);
+        
+        motorL.speed(lspeed);
+        motorR.speed(rspeed);
+        
+        lcd_.WriteValueXY("%1.2f",lspeed,3,1);
+        lcd_.WriteValueXY("%1.2f",rspeed,8,1);
+        
+         
+         bflag_up_pre   = bflag_up_cur;
+         bflag_down_pre = bflag_down_cur;
+    }
+    
+
 
+void DisplayStatus()
+                {
+                lcd_.WriteValueXY("T%3d ",shaft_deg, 0,0);
+                lcd_.WriteValue("V%2d",shaft_speed);
+                
+                lcd_.WriteStringXY("F",9,0);
+                    for (int i=0;i<4;i++){
+                        lcd_.WriteValue("%d",sw_in[i]);
+                    }
+                }
+                                
+int MainIOloop()
+{
+              static int cnt=0;
+              //iqs62x.waitForIqsReady();
+              iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers           
+              shaft_deg = ReadDegree(iqs62x.registers)-initial_deg;
+              if(shaft_deg<0)
+              {
+                shaft_deg = shaft_deg+360; 
+               }
+               
+              shaft_speed= ReadSpeed(iqs62x.registers);
+              //lcd_.WriteValueXY("%3d ",k, 0,0);
+              
+              for (int i=0;i<4;i=i++){
+                  sw_in[i]=!in_switchs[i];
+                  //sw_in[i+1]=!in_switchs[i+1];
+              }
+              cnt++;
+              return cnt;    
+}
+    
+
+void TimerIsr()
+{
+    //For LED Time-Sec display
+    //wait_ms(5);
+    g_timer++;
+    //Displaylevel(val);
+}
+
+
+
+
+void flip() {
+    static bool b = false;
+     
+    if(b==false)
+    {
+        timer_.attach(&TimerIsr, TIMER_COUNT);
+    }
+    
+    else
+    {
+        timer_.detach();
+        //Relay1=0;
+        sp_index++;
+        if (sp_index == 3)
+        {
+            sp_index = 0;    
+        }
+    }
+    b=!b;
+    
+}
 
 int ReadDegree(char * buffer)
 {
     int ret=0;   
-    
     //(High bit + Low bit) * 360/65536 
     //ret = ((buffer[0x80]<<8 + buffer[0x81])*0.00549316406  ;  
     ret = (buffer[0x80]<<8 +buffer[0x81])/65536.0*360.0  ;  
     return ret;   
 }
-    
 
+  
 int ReadSpeed(char * buffer)
 {
     int ret=0;   
@@ -90,197 +283,8 @@
        { 
         lcd_.WriteString(" ");          
         }
-    }
-    
-            
-    
-}
-    
-
-void TimerIsr()
-{
-    
-    //For LED Time-Sec display
-    static int k = 0;
-    static char ctext[4]="---";
-    div_t d_Cycle = div (k, SWITCH_PERIOD);
-    
-    //for Current time
-    div_t d_sec = div(k,600*2);    //60s * 10n
-    int t_min = d_sec.quot;
-    div_t d_min = div(t_min,60);   //1min=60s
-    int t_hr  = d_min.quot;
-    
-    //for Current time
-    div_t df_sec = div(TOTAL_TIMES*SWITCH_PERIOD,600);    //60s * 10n
-    int tf_min = df_sec.quot;
-    div_t df_min = div(tf_min,60);   //1min=60s
-    int tf_hr  = df_min.quot;
-    
-    
-    //Motor activation
-    
-    //Up Movement
-    if(WAIT_DELAY <= d_Cycle.rem && d_Cycle.rem < (WAIT_DELAY+UP_DURATION) )
-        {
-          int accel_index = d_Cycle.rem -  WAIT_DELAY;
-          if (accel_index < ACCEL_SIZE)
-            {
-              motor_speed=vol_accel[accel_index] * spd_table[sp_index];
-            }
-            else
-            {
-              motor_speed=spd_table[sp_index];
-            }
-          strcpy(ctext," CW");
-         //ctext="CW";
-         
-        }
-        
-        //UP..stop
-    else if ( (WAIT_DELAY+UP_DURATION)<= d_Cycle.rem && d_Cycle.rem <(2*WAIT_DELAY+UP_DURATION)   )
-        {
-          //wait_ms(20);
-          int accel_index = d_Cycle.rem - (WAIT_DELAY+UP_DURATION);
-          if (accel_index< DECEL_SIZE)
-            {
-              motor_speed=vol_decel[accel_index] * spd_table[sp_index];
-            }
-            else
-            {
-              motor_speed=0;
-            }
-          strcpy(ctext,"OFF");
-        }
-        
-        
-        //down..Start 
-    else if ( (2*WAIT_DELAY+UP_DURATION) <= d_Cycle.rem && d_Cycle.rem <(2*WAIT_DELAY+UP_DURATION+DOWN_DURATION)  )
-        {
-          int accel_index = d_Cycle.rem -  (2*WAIT_DELAY+UP_DURATION);
-          if (accel_index < ACCEL_SIZE)
-            {
-              motor_speed= - vol_accel[accel_index] * spd_table[sp_index]*donw_ratio;
-            }
-            else
-            {
-              motor_speed= -spd_table[sp_index]*donw_ratio;
-            }
-          strcpy(ctext," CW");
-         //ctext="CW";
-        }
-        
-        
-        //down..stop
-    else if ( (2*WAIT_DELAY+UP_DURATION+DOWN_DURATION) <= d_Cycle.rem  )
-        {
-          //wait_ms(20);
-          int accel_index = d_Cycle.rem - ((2*WAIT_DELAY+UP_DURATION+DOWN_DURATION));
-          if (accel_index< DECEL_SIZE)
-            {
-              motor_speed= -vol_decel[accel_index] * spd_table[sp_index]*donw_ratio;
-            }
-            else
-            {
-              motor_speed=0;
-            }
-          strcpy(ctext,"OFF");
-        }
-        
-        
-    if(d_Cycle.quot==TOTAL_TIMES)
-    {
-        timer_.detach();    
-    }
-    
-
-                             
-/*
-    char str[20];
-    sprintf(str, "%d'%2d\"", msec.quot, msec.rem);
-    lcd_.WriteStringXY(str, 0, 1);
-*/
-
-    //1 Row
-    //lcd_.WriteStringXY("#",0,0);
-    
-    
-    
-    lcd_.WriteValueXY("%5d/", d_Cycle.quot,0,0);
-    lcd_.WriteValue("%0dM",TOTAL_TIMES);
-    lcd_.WriteValue("%1.2f", motor_speed);
-    //lcd_.WriteValue("V%0d",motor_speed);
-    
-    
-    //2 Row
-    lcd_.WriteValueXY("%03dh", t_hr, 0, 1);
-    lcd_.WriteValue("%02dm",   d_min.rem);
-    lcd_.WriteValue("%03ds/",  d_sec.rem);
-    lcd_.WriteValue("%03dh", tf_hr);
-    lcd_.WriteValue("%02dm",   df_min.rem);
-    
-    k++;        
+    }        
 }
 
 
 
-void flip() {
-    static bool b = false;
-     
-    if(b==false)
-    {
-        timer_.attach(&TimerIsr, 0.05);
-    }
-    
-    else
-    {
-        timer_.detach();
-        //Relay1=0;
-        sp_index++;
-        if (sp_index == 3)
-        {
-            sp_index = 0;    
-        }
-    }
-    b=!b;
-    
-}
-
-
-int main()
-{
-
-    //motor.speed(0);
-    //  Check error and reset
-      
-    //LCD_cont=0;
-    //if (lcd_.IsConnected()) printf("\r\nConnected");
-    //else                    printf("\r\nDisconnected");
-
-    //TimerIsr();
-    //timer_.attach(&TimerIsr, 0.1);
-    //button1.fall(&flip);
-    //lcd_.WriteStringXY("0",0,0);
-    lcd_.WriteStringXY("Boot1",0,0);
-    iqs62x.configure(); // configure the ICD
-    lcd_.WriteStringXY("Boot2",0,0);
-    
-//    bool status = motor.status();
-//   if (status & DRV8830_F_FAULT){
-//        motor.reset();
-//     } 
-    
-    while (true) {
-            iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers        
-            int val = ReadDegree(iqs62x.registers);
-            int speed= ReadSpeed(iqs62x.registers);
-            
-            lcd_.WriteValueXY("T:%3d ",val, 0,1);
-            lcd_.WriteValue("V:%3d",speed);
-            Displaylevel(val);
-            wait_ms(5);
-            
-    }
-            
-}
-