File content as of revision 10:ef379cbc0004:

//------------------------------------------------------------
// Demo program for LCD ACM1602NI using I2C interface
//      Pullup resistors for SDA and SCL: 4.7 kΩ
// 2016/04/01, Copyright (c) 2016 MIKAMI, Naoki
//------------------------------------------------------------

//#define ANGLE_ENABLE
#include "ACM1602NI.hpp"
#include "DRV8830.h"
#include "IQS62x.h"
#include "IQSdisplayTerminal.h"
#include "MotorMove.h"

//Cycle 
#define UP_DURATION    14  //On time   [*100ms]
#define WAIT_DELAY     10  //Delay time [*100ms]
#define DOWN_DURATION   8  //Down time [*50ms]
#define SWITCH_PERIOD 50  //Cycle time[*50ms]
#define TOTAL_TIMES 30000   //total times n
#define TIMER_COUNT 0.01
#define LOOP_WAITMS 50

#define DEGREE_SHIFT    100
#define UP_THRESHOLD    10
#define DOWN_THRESHOLD  30


Ticker timer_;

using namespace Mikami;
Acm1602Ni lcd_;                               // Default, OK
//Acm1602Ni lcd_(D14, D15);                     // OK
//Acm1602Ni lcd_(D14, D15, 200000);             // OK
//Acm1602Ni lcd_(D14, D15, 200000, true, true); // OK
//Acm1602Ni lcd_(PB_3, PB_10);                  // OK
//Acm1602Ni lcd_(PC_9, PA_8);                   // OK
//Acm1602Ni lcd_(PB_4, PA_8);                   // OK 

I2C     i2c(D14, D15);
MotorMove mvalL;
MotorMove mvalR;

#ifdef ANGLE_ENABLE
IQS62xIO iqs62x;          // class for basic IQS62x block read and write
#endif
DigitalIn button1(USER_BUTTON);


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;

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

    
    
    //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;


//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 (shaft_deg< (DOWN_THRESHOLD+DEGREE_SHIFT) )
          { 
             bflag_down_cur=1;
           }
        else
          {
            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, 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]);
        
        
        motorL.speed(lspeed);
        wait_ms(1);
        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);
        
         
         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;
              #ifdef ANGLE_ENABLE
              iqs62x.waitForIqsReady();
              iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers           
              shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+ DEGREE_SHIFT;
              #endif
              
              
              if(shaft_deg<0)
              {
                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]);
              }
              */
              


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

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;   
    ret = (buffer[0x8E]);  
    return ret;   
}

void Displaylevel (int deg)
{
    
    int level=deg>>5;
    lcd_.WriteStringXY("@",0,0);
    for (int i=0;i<12;i++)
    {  
       if (i<level)
       {
         lcd_.WriteString("-");          
        }
       else
       { 
        lcd_.WriteString(" ");          
        }
    }        
}