File content as of revision 12:8464be95bf76:

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


//For Flag if we need sample or not
//#define ANGLE_ENABLE
#define SERIAL_ENABLE
//#define LED_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 MIN_ANGLE -90


#define LOOP_WAITMS 50
#define SPEED_RATIO 0.5

#define UP_THRESHOLD    80
#define DOWN_THRESHOLD  70


#define NUM_LED 3
#define MAX_LED_ANGLE 100
#define MAX_LED_BRI 0.7
#define HSV_SHIFT 0.0

/*
#define L_UP  .5
#define R_UP  .5
#define L_DOWN  .5
#define R_DOWN  .5
*/


#define L_UP  0.7
#define R_UP  0.7
#define L_DOWN  0.65
#define R_DOWN  0.65



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 SERIAL_ENABLE
Serial pc(USBTX, USBRX); // tx, rx
#endif

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


#ifdef LED_ENABLE
PwmOut  LEDPWM[NUM_LED]= {D3,D6,D9};
#endif

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;
static int degree_shift=0;

float pwmPeriod=0.000005;

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();
void ShowLED(int angle);
void HSV2RGB(const float *HSV, float *RGB);





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_Calibration",0,0);
    //wait(1);
    //iqs62x.configure(); // configure
    //wait(1);
    lcd_.WriteStringXY("IQS_Cconfig done",0,0);
    
    iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers      
    initial_deg = ReadDegree(iqs62x.registers);
    
    //degree_shift = (360- ReadDegree(iqs62x.registers) );


    #ifdef LED_ENABLE
    for (int i=0; i<NUM_LED ;i=i+1)
    {
        LEDPWM[i].period(pwmPeriod);
        LEDPWM[i]=1;
    }
    #endif
    
    wait(1);
    lcd_.WriteStringXY("IQS_Init_done ",0,0);
    #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();
                
                
                #ifdef SERIAL_ENABLE
                pc.printf("%d\r\n", shaft_deg);
                #endif
                
                
                /*
                LEDPWM[0]=1;
                LEDPWM[1]=1;
                LEDPWM[2]=1;
                */
                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( button1==1)
         {   
            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==0)
          {
               bflag_up_cur=0;
               bflag_down_cur=1;                          
          }
               
               
           
           
           
           #else 
           if (button1==1)
            {
               bflag_up_cur=1;
               bflag_down_cur=0;              
            }
            else
            {
               bflag_up_cur=0;
               bflag_down_cur=1;               
             } 
           #endif
           
           
           int mot_speed;
           
           if (shaft_speed==0)
           {
               mot_speed=1;
            }
            else
            {
               mot_speed = shaft_speed;
            }
               
               
           
//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, L_UP * mot_speed * SPEED_RATIO);
//                mvalR.up_motor_set(cnt, R_UP * mot_speed * SPEED_RATIO);
                mvalL.up_motor_set(cnt, L_UP);
                mvalR.up_motor_set(cnt, R_UP);
                lcd_.WriteStringXY("U",0,1);
              }
              
         else if(bflag_down_pre==0 && bflag_down_cur==1)
              {
//                mvalL.down_motor_set(cnt, L_DOWN * mot_speed *SPEED_RATIO);
//                mvalR.down_motor_set(cnt, R_DOWN * mot_speed *SPEED_RATIO );
                mvalL.down_motor_set(cnt, L_DOWN);
                mvalR.down_motor_set(cnt, R_DOWN);
                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*5,3,1);
        lcd_.WriteValueXY("%1.2f ",rspeed*5,8,1);
        
         
         bflag_up_pre   = bflag_up_cur;
         bflag_down_pre = bflag_down_cur;
    }
    
    
    
#ifdef LED_ENABLE
void ShowLED(int angle)
    {
        float led_H;
        float led_V;
        
        led_H = (float)angle/(float)MAX_LED_ANGLE+HSV_SHIFT;
        //led_val = (float)angle/(float)MAX_LED_ANGLE;
        led_V = (float)angle/(float)MAX_LED_ANGLE;
        

        if (led_H>1.0)
        {
           led_H = led_H - 1.0;
        }
        
        
        
        float RGB[3];
        float HSV[3];
        
        
        
        HSV[0]=led_H;
        HSV[1]=1;
        HSV[2]=MAX_LED_BRI;
        
        
        HSV2RGB(HSV,RGB);


        LEDPWM[0]= 1.0-RGB[0]*1.1;
        LEDPWM[1]= 1.0-RGB[1];
        LEDPWM[2]= 1.0-RGB[2];
        
        
        
    }
    
#endif
        
        


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;
                          
               
              shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+degree_shift;
              
              if (shaft_deg<MIN_ANGLE)
               {
                  shaft_deg=shaft_deg+360;
               }
              else if(shaft_deg>(MIN_ANGLE+360))
               {
                shaft_deg= shaft_deg-360;
                }
              /*
              if(shaft_deg<0)
              {
                shaft_deg = shaft_deg+360; // offset 100deg to cancel error 
               }
               */               
#endif
                             
              #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]);
              }
              */
              
              
              #ifdef LED_ENABLE
              ShowLED(shaft_deg);
              #endif


              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(" ");          
        }
    }      
    
}  
    
void HSV2RGB(const float *HSV, float *RGB) {
    float h = HSV[0];
    float s = HSV[1];
    float v = HSV[2];
    
    float r = v;
    float g = v;
    float b = v;
    
    if (s > 0.0f) {
        h *= 6.0f;
        int i = (int) h;
        float f = h - (float) i;
        switch (i) {
            default:
            case 0:
                g *= 1 - s * (1 - f);
                b *= 1 - s;
                break;
            case 1:
                r *= 1 - s * f;
                b *= 1 - s;
                break;
            case 2:
                r *= 1 - s;
                b *= 1 - s * (1 - f);
                break;
            case 3:
                r *= 1 - s;
                g *= 1 - s * f;
                break;
            case 4:
                r *= 1 - s * (1 - f);
                g *= 1 - s;
                break;
            case 5:
                g *= 1 - s;
                b *= 1 - s * f;
                break;
        }
    }
        
    RGB[0] = r;
    RGB[1] = g;
    RGB[2] = b;
}