Diff: main.cpp

Revision:

8:f7ad1d7176ba

Parent:

7:acb1074eaba6

Child:

9:b58e7d72a91c

--- 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);
-            
-    }
-            
-}
-