Diff: main.cpp

Revision:

9:b58e7d72a91c

Parent:

8:f7ad1d7176ba

Child:

10:ef379cbc0004

--- a/main.cpp	Sat Sep 30 16:43:51 2017 +0000
+++ b/main.cpp	Mon Oct 02 19:12:02 2017 +0000
@@ -4,6 +4,7 @@
 // 2016/04/01, Copyright (c) 2016 MIKAMI, Naoki
 //------------------------------------------------------------
 
+//#define ANGLE_ENABLE
 #include "ACM1602NI.hpp"
 #include "DRV8830.h"
 #include "IQS62x.h"
@@ -17,10 +18,11 @@
 #define SWITCH_PERIOD 50  //Cycle time[*50ms]
 #define TOTAL_TIMES 30000   //total times n
 #define TIMER_COUNT 0.01
-#define LOOP_WAITMS 30
+#define LOOP_WAITMS 50
 
+#define DEGREE_SHIFT    100
 #define UP_THRESHOLD    10
-#define DOWN_THRESHOLD  90
+#define DOWN_THRESHOLD  30
 
 
 Ticker timer_;
@@ -35,13 +37,13 @@
 //Acm1602Ni lcd_(PB_4, PA_8);                   // OK 
 
 I2C     i2c(D14, D15);
-DRV8830 motorL(i2c, DRV8830ADDR_NN);   //Motor1
-DRV8830 motorR(i2c, DRV8830ADDR_0N);   //Motor2
 MotorMove mvalL;
 MotorMove mvalR;
 
+#ifdef ANGLE_ENABLE
 IQS62xIO iqs62x;          // class for basic IQS62x block read and write
-InterruptIn button1(USER_BUTTON);
+#endif
+DigitalIn button1(USER_BUTTON);
 
 
 DigitalIn in_switchs[]=
@@ -57,6 +59,9 @@
 
 static int initial_deg=0;
 
+DRV8830 motorL(i2c, DRV8830ADDR_NN);   //Motor1
+DRV8830 motorR(i2c, DRV8830ADDR_0N);   //Motor2
+
 
 void ShowLCD(char * buffer,  int startbyte, int endbyte); // for wheel output
 int ReadDegree(char * buffer);
@@ -78,21 +83,32 @@
               
     //motor.speed(0);
     //Initialize Ic2 Device
+
+
     motorL.speed(0);
     motorR.speed(0);
     
+    
+    
+    #ifdef ANGLE_ENABLE
     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);
+    #else
+    lcd_.WriteStringXY("No_Sensor",0,0);
+    initial_deg=0;
+    #endif
+    
+    
 
     
-    lcd_.WriteStringXY("IQS_Init_done",0,0);
-    wait(1);
     
     //read 0deg for initialize
-    button1.fall(&flip);
+    //button1.fall(&flip);
 
 //TimerIsr();
 //timer_.attach(&TimerIsr, TIMER_COUNT);
@@ -102,7 +118,7 @@
 //     } 
    
 //Read here as Asynchronous when data gets ready
-while (true) {             
+while (true)   {
                 int time_current = g_timer;
                 int time_diff = time_current - time_prev;
                 int a=  MainIOloop();     
@@ -112,11 +128,8 @@
                 //motorR.speed( (shaft_deg-180.0)/200.0 );
                 wait_ms(LOOP_WAITMS);
                 MoveMotor();
-                                
                 cnt ++;
-                }
-            
-            
+                }    
 }
 
 void MoveMotor(){
@@ -128,8 +141,9 @@
          float rspeed;
 
 
-//detect up or donw by thredold        
-         if (shaft_deg>UP_THRESHOLD)
+//detect up or donw by thredold
+         #ifdef ANGLE_ENABLE
+         if (shaft_deg> (UP_THRESHOLD+DEGREE_SHIFT) )
           { 
              bflag_up_cur=1;
            }
@@ -138,7 +152,7 @@
             bflag_up_cur=0;
            };
          
-         if (shaft_deg<DOWN_THRESHOLD)
+         if (shaft_deg< (DOWN_THRESHOLD+DEGREE_SHIFT) )
           { 
              bflag_down_cur=1;
            }
@@ -146,33 +160,45 @@
           {
             bflag_down_cur=0;
            };
-           
+           #else 
+           if (button1==1)
+            {
+               bflag_up_cur=1;
+               bflag_down_cur=0;              
+            }
+            else
+            {
+               bflag_up_cur=0;
+               bflag_down_cur=1;               
+             } 
+           #endif
            
 //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);
+                mvalL.up_motor_set(cnt, 0.9);
+                mvalR.up_motor_set(cnt, 0.9);
                 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]);
+        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);
+        //motorL.speed(0.5);
+        //motorR.speed(0.3);
         
         lcd_.WriteValueXY("%1.2f",lspeed,3,1);
         lcd_.WriteValueXY("%1.2f",rspeed,8,1);
@@ -198,22 +224,53 @@
 int MainIOloop()
 {
               static int cnt=0;
-              //iqs62x.waitForIqsReady();
+              #ifdef ANGLE_ENABLE
+              iqs62x.waitForIqsReady();
               iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers           
-              shaft_deg = ReadDegree(iqs62x.registers)-initial_deg;
+              shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+ DEGREE_SHIFT;
+              #endif
+              
+              
               if(shaft_deg<0)
               {
-                shaft_deg = shaft_deg+360; 
+                shaft_deg = shaft_deg+360; // offset 100deg to cancel error 
                }
                
+               
+              #ifdef ANGLE_ENABLE
               shaft_speed= ReadSpeed(iqs62x.registers);
               //lcd_.WriteValueXY("%3d ",k, 0,0);
+              #endif
               
+              sw_in[0]= in_switchs[0];
+              sw_in[1]= !in_switchs[1];
+              sw_in[2]= in_switchs[2];
+              sw_in[3]= ! in_switchs[3];
+              
+/*
               for (int i=0;i<4;i=i++){
                   sw_in[i]=!in_switchs[i];
-                  //sw_in[i+1]=!in_switchs[i+1];
+                  sw_in[i+1]=!(in_switchs[i+1]);
               }
+              */
+              
+
+
               cnt++;
+              
+            
+            bool statusL = motorL.status();
+                if (statusL & DRV8830_F_FAULT){
+                motorL.reset();
+            }
+            
+            bool statusR = motorR.status();
+                if (statusR & DRV8830_F_FAULT){
+                motorR.reset();
+            }
+            
+            
+             
               return cnt;    
 }
     
@@ -227,8 +284,6 @@
 }
 
 
-
-
 void flip() {
     static bool b = false;