main.cpp

00001 //------------------------------------------------------------
00002 // Demo program for LCD ACM1602NI using I2C interface
00003 //      Pullup resistors for SDA and SCL: 4.7 kΩ
00004 // 2016/04/01, Copyright (c) 2016 MIKAMI, Naoki
00005 //------------------------------------------------------------
00006 
00007 
00008 //For Flag if we need sample or not
00009 //#define ANGLE_ENABLE
00010 #define SERIAL_ENABLE
00011 //#define LED_ENABLE
00012 
00013 #include "ACM1602NI.hpp"
00014 #include "DRV8830.h"
00015 #include "IQS62x.h"
00016 #include "IQSdisplayTerminal.h"
00017 #include "MotorMove.h"
00018 
00019 //Cycle 
00020 #define UP_DURATION    14  //On time   [*100ms]
00021 #define WAIT_DELAY     10  //Delay time [*100ms]
00022 #define DOWN_DURATION   8  //Down time [*50ms]
00023 #define SWITCH_PERIOD 50  //Cycle time[*50ms]
00024 #define TOTAL_TIMES 30000   //total times n
00025 #define TIMER_COUNT 0.01
00026 #define MIN_ANGLE -90
00027 
00028 
00029 #define LOOP_WAITMS 50
00030 #define SPEED_RATIO 0.5
00031 
00032 #define UP_THRESHOLD    80
00033 #define DOWN_THRESHOLD  70
00034 
00035 
00036 #define NUM_LED 3
00037 #define MAX_LED_ANGLE 100
00038 #define MAX_LED_BRI 0.7
00039 #define HSV_SHIFT 0.0
00040 
00041 /*
00042 #define L_UP  .5
00043 #define R_UP  .5
00044 #define L_DOWN  .5
00045 #define R_DOWN  .5
00046 */
00047 
00048 
00049 #define L_UP  0.7
00050 #define R_UP  0.7
00051 #define L_DOWN  0.65
00052 #define R_DOWN  0.65
00053 
00054 
00055 
00056 Ticker timer_;
00057 
00058 using namespace Mikami;
00059 Acm1602Ni lcd_;                               // Default, OK
00060 //Acm1602Ni lcd_(D14, D15);                     // OK
00061 //Acm1602Ni lcd_(D14, D15, 200000);             // OK
00062 //Acm1602Ni lcd_(D14, D15, 200000, true, true); // OK
00063 //Acm1602Ni lcd_(PB_3, PB_10);                  // OK
00064 //Acm1602Ni lcd_(PC_9, PA_8);                   // OK
00065 //Acm1602Ni lcd_(PB_4, PA_8);                   // OK 
00066 
00067 I2C     i2c(D14, D15);
00068 MotorMove mvalL;
00069 MotorMove mvalR;
00070 
00071 #ifdef SERIAL_ENABLE
00072 Serial pc(USBTX, USBRX); // tx, rx
00073 #endif
00074 
00075 #ifdef ANGLE_ENABLE
00076 IQS62xIO iqs62x;          // class for basic IQS62x block read and write
00077 #endif
00078 DigitalIn button1(USER_BUTTON);
00079 
00080 
00081 #ifdef LED_ENABLE
00082 PwmOut  LEDPWM[NUM_LED]= {D3,D6,D9};
00083 #endif
00084 
00085 DigitalIn in_switchs[]=
00086    {   DigitalIn(A0), DigitalIn(A1), DigitalIn(A2), DigitalIn(A3)};
00087 
00088 static int shaft_deg=0;
00089 static int shaft_speed=0;  
00090 static int g_timer=0;              //gloabl timer
00091 static int cnt;   //total timer of loop 
00092 static int sw_in[4]={0,0,0,0};     //swithc flag bit
00093 static int sp_index=0; //Movement mode 0-3
00094 
00095 static int initial_deg=0;
00096 static int degree_shift=0;
00097 
00098 float pwmPeriod=0.000005;
00099 
00100 DRV8830 motorL(i2c, DRV8830ADDR_NN);   //Motor1
00101 DRV8830 motorR(i2c, DRV8830ADDR_0N);   //Motor2
00102 
00103 
00104 void ShowLCD(char * buffer,  int startbyte, int endbyte); // for wheel output
00105 int ReadDegree(char * buffer);
00106 int ReadSpeed(char * buffer);
00107 void Displaylevel (int deg);
00108 void TimerIsr();
00109 void flip();
00110 int  MainIOloop();
00111 void DisplayStatus();
00112 void MoveMotor();
00113 void ShowLED(int angle);
00114 void HSV2RGB(const float *HSV, float *RGB);
00115 
00116 
00117 
00118 
00119 
00120 int main()
00121 {
00122    //initialize system    
00123    static int time_prev;
00124    for (int i=0;i<4;i=i++){
00125        in_switchs[i].mode(PullUp);
00126        }
00127               
00128     //motor.speed(0);
00129     //Initialize Ic2 Device
00130 
00131 
00132     //motorL.speed(0);
00133     //motorR.speed(0);
00134     
00135     
00136     
00137     #ifdef ANGLE_ENABLE
00138     lcd_.WriteStringXY("IQS_Calibration",0,0);
00139     //wait(1);
00140     //iqs62x.configure(); // configure
00141     //wait(1);
00142     lcd_.WriteStringXY("IQS_Cconfig done",0,0);
00143     
00144     iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers      
00145     initial_deg = ReadDegree(iqs62x.registers);
00146     
00147     //degree_shift = (360- ReadDegree(iqs62x.registers) );
00148 
00149 
00150     #ifdef LED_ENABLE
00151     for (int i=0; i<NUM_LED ;i=i+1)
00152     {
00153         LEDPWM[i].period(pwmPeriod);
00154         LEDPWM[i]=1;
00155     }
00156     #endif
00157     
00158     wait(1);
00159     lcd_.WriteStringXY("IQS_Init_done ",0,0);
00160     #else
00161     lcd_.WriteStringXY("No_Sensor",0,0);
00162     initial_deg=0;
00163     #endif
00164     
00165 
00166 
00167     
00168     
00169     //read 0deg for initialize
00170     //button1.fall(&flip);
00171 
00172 //TimerIsr();
00173 //timer_.attach(&TimerIsr, TIMER_COUNT);
00174 //    bool status = motor.status();
00175 //   if (status & DRV8830_F_FAULT){
00176 //        motor.reset();
00177 //     } 
00178    
00179 //Read here as Asynchronous when data gets ready
00180 while (true)   {
00181                 int time_current = g_timer;
00182                 int time_diff = time_current - time_prev;
00183                 int a=  MainIOloop();     
00184                 DisplayStatus();                
00185                 //display_info
00186                 time_prev = time_current;
00187                 //motorR.speed( (shaft_deg-180.0)/200.0 );
00188                 wait_ms(LOOP_WAITMS);
00189                 MoveMotor();
00190                 
00191                 
00192                 #ifdef SERIAL_ENABLE
00193                 pc.printf("%d\r\n", shaft_deg);
00194                 #endif
00195                 
00196                 
00197                 /*
00198                 LEDPWM[0]=1;
00199                 LEDPWM[1]=1;
00200                 LEDPWM[2]=1;
00201                 */
00202                 cnt ++;
00203                 }    
00204 }
00205 
00206 void MoveMotor(){
00207          static int bflag_up_pre=0;
00208          static int bflag_down_pre=0;
00209          int bflag_up_cur  =0;
00210          int bflag_down_cur =0;
00211          float lspeed;
00212          float rspeed;
00213 
00214 
00215 //detect up or donw by thredold
00216          #ifdef ANGLE_ENABLE
00217          
00218          
00219          
00220          if( button1==1)
00221          {   
00222             if (shaft_deg>      (UP_THRESHOLD+degree_shift))
00223                 { 
00224                   bflag_up_cur=1;
00225                 }
00226             else
00227                 {
00228                   bflag_up_cur=0;
00229                 };
00230            
00231          
00232              if (shaft_deg< (DOWN_THRESHOLD+degree_shift) )
00233                { 
00234                  bflag_down_cur=1;
00235                }
00236               else
00237                {
00238                  bflag_down_cur=0;
00239                }
00240                
00241           }
00242                
00243                
00244                
00245         else if (button1==0)
00246           {
00247                bflag_up_cur=0;
00248                bflag_down_cur=1;                          
00249           }
00250                
00251                
00252            
00253            
00254            
00255            #else 
00256            if (button1==1)
00257             {
00258                bflag_up_cur=1;
00259                bflag_down_cur=0;              
00260             }
00261             else
00262             {
00263                bflag_up_cur=0;
00264                bflag_down_cur=1;               
00265              } 
00266            #endif
00267            
00268            
00269            int mot_speed;
00270            
00271            if (shaft_speed==0)
00272            {
00273                mot_speed=1;
00274             }
00275             else
00276             {
00277                mot_speed = shaft_speed;
00278             }
00279                
00280                
00281            
00282 //send down or up command when status had changed
00283          if(bflag_up_pre==0&& bflag_up_cur==1)
00284               {
00285                 //shaft_speed
00286 //                mvalL.up_motor_set(cnt, L_UP * mot_speed * SPEED_RATIO);
00287 //                mvalR.up_motor_set(cnt, R_UP * mot_speed * SPEED_RATIO);
00288                 mvalL.up_motor_set(cnt, L_UP);
00289                 mvalR.up_motor_set(cnt, R_UP);
00290                 lcd_.WriteStringXY("U",0,1);
00291               }
00292               
00293          else if(bflag_down_pre==0 && bflag_down_cur==1)
00294               {
00295 //                mvalL.down_motor_set(cnt, L_DOWN * mot_speed *SPEED_RATIO);
00296 //                mvalR.down_motor_set(cnt, R_DOWN * mot_speed *SPEED_RATIO );
00297                 mvalL.down_motor_set(cnt, L_DOWN);
00298                 mvalR.down_motor_set(cnt, R_DOWN);
00299                 lcd_.WriteStringXY("D",1,1);
00300               } 
00301         else{
00302                lcd_.WriteStringXY("__",0,1);
00303             }
00304         
00305         lspeed=    mvalL.ReturnMotorVol(cnt, sw_in[0],sw_in[1]);
00306         rspeed=   -mvalR.ReturnMotorVol(cnt, sw_in[2],sw_in[3]);
00307         
00308         
00309         motorL.speed(lspeed);
00310         wait_ms(1);
00311         motorR.speed(rspeed);
00312         //motorL.speed(0.5);
00313         //motorR.speed(0.3);
00314         
00315         lcd_.WriteValueXY("%1.2f ",lspeed*5,3,1);
00316         lcd_.WriteValueXY("%1.2f ",rspeed*5,8,1);
00317         
00318          
00319          bflag_up_pre   = bflag_up_cur;
00320          bflag_down_pre = bflag_down_cur;
00321     }
00322     
00323     
00324     
00325 #ifdef LED_ENABLE
00326 void ShowLED(int angle)
00327     {
00328         float led_H;
00329         float led_V;
00330         
00331         led_H = (float)angle/(float)MAX_LED_ANGLE+HSV_SHIFT;
00332         //led_val = (float)angle/(float)MAX_LED_ANGLE;
00333         led_V = (float)angle/(float)MAX_LED_ANGLE;
00334         
00335 
00336         if (led_H>1.0)
00337         {
00338            led_H = led_H - 1.0;
00339         }
00340         
00341         
00342         
00343         float RGB[3];
00344         float HSV[3];
00345         
00346         
00347         
00348         HSV[0]=led_H;
00349         HSV[1]=1;
00350         HSV[2]=MAX_LED_BRI;
00351         
00352         
00353         HSV2RGB(HSV,RGB);
00354 
00355 
00356         LEDPWM[0]= 1.0-RGB[0]*1.1;
00357         LEDPWM[1]= 1.0-RGB[1];
00358         LEDPWM[2]= 1.0-RGB[2];
00359         
00360         
00361         
00362     }
00363     
00364 #endif
00365         
00366         
00367 
00368 
00369 void DisplayStatus()
00370                 {
00371                 lcd_.WriteValueXY("T%3d ",shaft_deg, 0,0);
00372                 lcd_.WriteValue("V%2d",shaft_speed);
00373                 
00374                 lcd_.WriteStringXY("F",9,0);
00375                     for (int i=0;i<4;i++){
00376                         lcd_.WriteValue("%d",sw_in[i]);
00377                     }
00378                 }
00379                                 
00380 int MainIOloop()
00381 {
00382               static int cnt=0;
00383               #ifdef ANGLE_ENABLE
00384               //iqs62x.waitForIqsReady();
00385               iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers           
00386               //shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+ DEGREE_SHIFT;
00387                           
00388                
00389               shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+degree_shift;
00390               
00391               if (shaft_deg<MIN_ANGLE)
00392                {
00393                   shaft_deg=shaft_deg+360;
00394                }
00395               else if(shaft_deg>(MIN_ANGLE+360))
00396                {
00397                 shaft_deg= shaft_deg-360;
00398                 }
00399               /*
00400               if(shaft_deg<0)
00401               {
00402                 shaft_deg = shaft_deg+360; // offset 100deg to cancel error 
00403                }
00404                */               
00405 #endif
00406                              
00407               #ifdef ANGLE_ENABLE
00408               shaft_speed= ReadSpeed(iqs62x.registers);
00409               //lcd_.WriteValueXY("%3d ",k, 0,0);
00410               #endif
00411               
00412               sw_in[0]= in_switchs[0];
00413               sw_in[1]= !in_switchs[1];
00414               sw_in[2]= in_switchs[2];
00415               sw_in[3]= ! in_switchs[3];
00416               
00417 /*
00418               for (int i=0;i<4;i=i++){
00419                   sw_in[i]=!in_switchs[i];
00420                   sw_in[i+1]=!(in_switchs[i+1]);
00421               }
00422               */
00423               
00424               
00425               #ifdef LED_ENABLE
00426               ShowLED(shaft_deg);
00427               #endif
00428 
00429 
00430               cnt++;
00431               
00432             
00433             bool statusL = motorL.status();
00434                 if (statusL & DRV8830_F_FAULT){
00435                 motorL.reset();
00436             }
00437             
00438             bool statusR = motorR.status();
00439                 if (statusR & DRV8830_F_FAULT){
00440                 motorR.reset();
00441             }
00442             
00443             
00444              
00445               return cnt;    
00446 }
00447     
00448 
00449 void TimerIsr()
00450 {
00451     //For LED Time-Sec display
00452     //wait_ms(5);
00453     g_timer++;
00454     //Displaylevel(val);
00455 }
00456 
00457 
00458 void flip() {
00459     static bool b = false;
00460      
00461     if(b==false)
00462     {
00463         timer_.attach(&TimerIsr, TIMER_COUNT);
00464     }
00465     
00466     else
00467     {
00468         timer_.detach();
00469         //Relay1=0;
00470         sp_index++;
00471         if (sp_index == 3)
00472         {
00473             sp_index = 0;    
00474         }
00475     }
00476     b=!b;
00477     
00478 }
00479 
00480 int ReadDegree(char * buffer)
00481 {
00482     int ret=0;   
00483     //(High bit + Low bit) * 360/65536 
00484     //ret = ((buffer[0x80]<<8 + buffer[0x81])*0.00549316406  ;  
00485     ret = (buffer[0x80]<<8 +buffer[0x81])/65536.0*360.0  ;  
00486     return ret;   
00487 }
00488 
00489   
00490 int ReadSpeed(char * buffer)
00491 {
00492     int ret=0;   
00493     ret = (buffer[0x8E]);  
00494     return ret;   
00495 }
00496 
00497 void Displaylevel (int deg)
00498 {
00499     
00500     int level=deg>>5;
00501     lcd_.WriteStringXY("@",0,0);
00502     for (int i=0;i<12;i++)
00503     {  
00504        if (i<level)
00505        {
00506          lcd_.WriteString("-");          
00507         }
00508        else
00509        { 
00510         lcd_.WriteString(" ");          
00511         }
00512     }      
00513     
00514 }  
00515     
00516 void HSV2RGB(const float *HSV, float *RGB) {
00517     float h = HSV[0];
00518     float s = HSV[1];
00519     float v = HSV[2];
00520     
00521     float r = v;
00522     float g = v;
00523     float b = v;
00524     
00525     if (s > 0.0f) {
00526         h *= 6.0f;
00527         int i = (int) h;
00528         float f = h - (float) i;
00529         switch (i) {
00530             default:
00531             case 0:
00532                 g *= 1 - s * (1 - f);
00533                 b *= 1 - s;
00534                 break;
00535             case 1:
00536                 r *= 1 - s * f;
00537                 b *= 1 - s;
00538                 break;
00539             case 2:
00540                 r *= 1 - s;
00541                 b *= 1 - s * (1 - f);
00542                 break;
00543             case 3:
00544                 r *= 1 - s;
00545                 g *= 1 - s * f;
00546                 break;
00547             case 4:
00548                 r *= 1 - s * (1 - f);
00549                 g *= 1 - s;
00550                 break;
00551             case 5:
00552                 g *= 1 - s;
00553                 b *= 1 - s * f;
00554                 break;
00555         }
00556     }
00557         
00558     RGB[0] = r;
00559     RGB[1] = g;
00560     RGB[2] = b;
00561 }
00562     
00563     
00564     
00565