hidaka sato / Mbed 2 deprecated 2015_denziben_i2c_S2

anal

Dependencies:   Motor PID mbed

Fork of 2015_denziben_i2c_S by hidaka sato

Files at this revision

API Documentation at this revision

Revision 3:1194c29429bf, committed 2015-07-21

Comitter:
kikoaac
Date:
Tue Jul 21 08:13:33 2015 +0000
Parent:
1:6f51a981ed09
Commit message:
MotorDrive
;

Changed in this revision

Motor.lib Show annotated file Show diff for this revision Revisions of this file
PID.lib Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
Registar.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/Motor.lib	Fri Jun 19 06:38:52 2015 +0000
+++ b/Motor.lib	Tue Jul 21 08:13:33 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/kikoaac/code/Motor/#1252cd3b995f
+http://developer.mbed.org/users/kikoaac/code/Motor/#4ab6e9768847

--- a/PID.lib	Fri Jun 19 06:38:52 2015 +0000
+++ b/PID.lib	Tue Jul 21 08:13:33 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/kikoaac/code/PID/#aac6180820a4
+http://developer.mbed.org/users/kikoaac/code/PID/#14176355508a

--- a/QEI.lib	Fri Jun 19 06:38:52 2015 +0000
+++ b/QEI.lib	Tue Jul 21 08:13:33 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/kikoaac/code/QEI2/#1f4e8614d0ed
+http://developer.mbed.org/users/kikoaac/code/QEI2/#6a38785d5f0c

--- a/Registar.h	Fri Jun 19 06:38:52 2015 +0000
+++ b/Registar.h	Tue Jul 21 08:13:33 2015 +0000
@@ -1,22 +1,21 @@
 #include "mbed.h"
 #include "Defines.h"
-#define Red 0x01
-#define Green 0x02
-#define Blue 0x04
+#define RED 0x01
+#define GREEN 0x02
+#define BLUE 0x04
 SPISlave *spi;
 I2CSlave *i2c;
 DigitalIn *Mode;
 BusIn *Address;
-BusOut LED(Red_Pin,Green_Pin,Blue_Pin);
 
 
 
 
 #define MotorMode        0x04
 #define MotorState        0x0e
-#define Who_am_I          0x0
-#define MotorPWM          0x0f
-#define MotorPWM2          0x10
+#define Who_am_I          0x2
+#define MotorPWM          0x10
+#define MotorPWM2          0xf
 #define MoterRevolutionL   0x11
 #define MoterRevolutionH   0x12
 #define MoterPulseL 0x13
@@ -24,16 +23,21 @@
 #define MoterSpeedL 0x15
 #define MoterSpeedH 0x16
 #define Automatic   0x0
-#define MotorP 0x23
-#define MotorI 0x24
-#define MotorD 0x25
+#define MotorP 0x30
+#define MotorI 0x31
+#define MotorD 0x32
 #define FinishAuto 0x00
-#define TargetSpeed 0x00
+#define TargetSpeed 0x23
 #define TargetAngle2 0x22
 #define TargetAngle 0x21
-#define TargetSpeed2 0x00
+#define TargetSpeed2 0x24
 #define PulsePerRev 0x18
 #define PulsePerRev2 0x19
+#define PulsePerRevolutionL 0x25
+#define PulsePerRevolutionH 0x26
+#define PulsePerSec 0x27
+#define PulsePerSec2 0x28
+#define PulsePerAngle 0x20
 #define RotateMode  0x17
 
 

--- a/main.cpp	Fri Jun 19 06:38:52 2015 +0000
+++ b/main.cpp	Tue Jul 21 08:13:33 2015 +0000
@@ -1,18 +1,87 @@
 // Reply to a SPI master as slave
-float GAIN_P = 4.5f; // 比例ｹﾞｲﾝ
-float GAIN_I = 3.7f; // 積分ｹﾞｲﾝ
-float GAIN_D = 1.0f;
+float GAIN_P = 4.3f; // 比例ｹﾞｲﾝ
+float GAIN_I = 1.3f; // 積分ｹﾞｲﾝ
+float GAIN_D = 1.21f;
  
 #include "mbed.h"
 #include "Motor.h"
 #include "QEI.h"
 #include "PID.h"
 #include "Defines.h"
-
-char Registar[0x80]={123};
+#include "Registar.h"
+BusOut LED(Red_Pin,Green_Pin,Blue_Pin);
+class LEDMode
+{
+    Ticker Tic;
+    private:
+        char led,Off;
+        void Inter()
+        {
+            static int flag;
+            flag++;
+            if(flag&2)
+            {
+                LED = led;
+            }
+            else
+            {
+                Tic.attach(this,&LEDMode::Inter,TransWait*0.3);
+                LED = Off; 
+            } 
+        }
+    public:
+        char Red,Blue,Green;
+        char TransWait;
+        char InterVal;
+        char Mode[7];// = {RED|BLUE,RED|GREEN,RED|GREEN,BLUE|GREEN,BLUE|GREEN};
+        float Int[7];
+        char OFF[7];
+        LEDMode()
+        {
+            char mode[7] = {RED,RED|BLUE,RED|GREEN,RED|GREEN,BLUE|GREEN,BLUE|GREEN,RED|BLUE};
+            char off[7] = {0,0,0,GREEN,0,GREEN,RED};
+            float in[7] = {1,1,1.5,1.7,1.5,1.7,1.0};
+            for(int i=0;i<7;i++)
+            {
+                Mode[i]=mode[i];
+                Int[i]=in[i];
+                OFF[i]=off[i];
+            }
+            Red=RED;
+            Blue=BLUE;
+            Green=GREEN;
+            Off=0;
+        }
+        void Wait(int i)
+        {
+            float in = 1.0;
+            TransWait = in;
+            if(i==0)led = BLUE;
+            else led = GREEN;
+            Off=0;
+            Tic.detach();
+            Tic.attach(this,&LEDMode::Inter,TransWait);
+        }
+        void Act(int i)
+        {
+            if(i>=6)i=6; 
+            {
+                Off=OFF[i];
+                led = Mode[i];
+                TransWait = Int[i];
+                Tic.detach();
+                Tic.attach(this,&LEDMode::Inter,TransWait);
+            }
+        }
+};
+LEDMode Led;
+PID pid(GAIN_P,GAIN_I,GAIN_D);
+short PR;
+short pulse ;
+short Rev;
+char Registar[0x80]={0};
 char mode;
-Timer timer;
-double PIctrl(double dCommand, double dVal)
+/*double PIctrl(double dCommand, double dVal)
  {
  static double s_dErrIntg = 0 ,dErr_prev=0;
  double dErr;
@@ -26,30 +95,31 @@
 
  // 制御入力
 dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*(dErr-dErr_prev)/timer.read();
+//printf("%f    ",timer.read());
 timer.reset();
+//printf("%f    ",dRet);
 dErr_prev = dErr;
  return (dRet);
-}
-PinName rotatepin[3];
+}*/
 Ticker rotateT;
-Ticker rotateN;
-QEI wheel(Rotal_A,Rotal_B,NC,100,QEI::X4_ENCODING);
+//Ticker rotateN;
+//QEI wheel(Rotal_A,Rotal_B,NC,100,QEI::X4_ENCODING);
 
     
-#include "Registar.h"
-Motor motor(Motor_H1,Motor_L2,Motor_L1,Motor_H2,Motor_PWM,1);
+Motor motor(Motor_H1,Motor_L2,Motor_L1,Motor_H2,Motor_PWM,short(0xffff));
 
-void ManiacMotor_Mode(short duty,char mode)
+void ManiacMotor_Mode(float duty,char mode)
 {
     motor.run(mode,duty);
     //motor=1;
+    //printf("%f %f\n " , duty,1.0/duty);
 }
 //PinName rotatepin[3];
 int rotatemode;
 void RotateSet()
 {
     //delete wheel;
-    short pulse = short(Registar[PulsePerRev]<<8|Registar[PulsePerRev2]);
+    //pulse = short(Registar[PulsePerRev]<<8|Registar[PulsePerRev2]);
     /*if(Registar[RotateMode]&0x01)rotatepin[0]=Rotal_A;
     //else rotatepin[0]=dp13;
     if(Registar[RotateMode]&0x02)rotatepin[1]=Rotal_B;
@@ -60,7 +130,6 @@
     else if(!Registar[RotateMode]&0x08)rotatemode=0;
     if(Registar[RotateMode]&0x80)
     { 
-        timer.start();
         //wheel.SetUp(rotatepin[0],rotatepin[1],rotatepin[2],pulse,rotatemode);
         //printf("make QEI\n");
     }
@@ -68,27 +137,32 @@
 void Rotate(/*void const *argument*/)
 {
     static bool flag=0;
-    if(Registar[RotateMode]&0x80)
+    
+    if(/*Registar[RotateMode]&0x80*/1)
     {
         if(!flag)
         {
             RotateSet();
             flag=1;
         }
-        short pulse = short(wheel.getPulses());
-        short Rev = short(wheel.getRevolutions());
-        short PR = short(wheel.getRPUS()*1000);
+        pulse = short((Registar[PulsePerRev]<<8)|Registar[PulsePerRev+1]/*wheel.getPulses()*/);
+        Rev = short((Registar[PulsePerRev]<<8)|Registar[PulsePerRev]/*wheel.getRevolutions()*/);
+        PR = short((Registar[PulsePerSec]<<8)|Registar[PulsePerSec+1]/*wheel.getRPMS()*1000*/);
         Registar[MoterRevolutionH] = (Rev&0xff00)>>8;
         Registar[MoterRevolutionH-1] = (Rev&0xff);
         Registar[MoterPulseH] = (pulse&0xff00)>>8;
         Registar[MoterPulseH-1] = (pulse&0xff);
         Registar[MoterSpeedH] = (PR&0xff00)>>8;
         Registar[MoterSpeedH-1] = (PR&0xff);
-        
-        GAIN_P = (float)Registar[MotorP]/10;
-        GAIN_I = (float)Registar[MotorI]/10;
-        GAIN_D = (float)Registar[MotorD]/10;
-        //printf("Rotate::: %d ,%d ,%d\n",pulse, Registar[MoterPulseH-1], Registar[MoterPulseH]);
+        char IF = Registar[MotorMode]&(~0x80);
+        if(IF == 3 || IF == 5)
+        {
+            pid.GAIN_P = (float)Registar[MotorP]/10;
+            pid.GAIN_I = (float)Registar[MotorI]/10;
+            pid.GAIN_D = (float)Registar[MotorD]/10;
+            //printf("PID P%f I%f D%f \n",pid.GAIN_P,pid.GAIN_I,pid.GAIN_D);
+        }
+        //printf("Rotate::: %d ,%d ,%d\n",pulse, Registar[PulsePerRev], Registar[PulsePerRev+1]);
     }
     /*if(flag==1)
     {
@@ -109,50 +183,114 @@
         motor.run(Back,1);
     else motor.run(Stop,1);
  
-    printf("%f %f %f \n",mypoint,point,sa);    
+    //printf("%f %f %f \n",mypoint,point,sa);    
 }
-void AngleStay_PID(float point , float mypoint)
+void AngleStay_PID(float point , float mypoint,bool mode)
 {
-    float x = PIctrl(point , mypoint);
+    //float x = PIctrl(point , mypoint);
+    float sa=0;
+    if(mode==1)    sa = (int)point%360-(int)mypoint%360;
+    else        sa = point-mypoint;
     //pid.dPoint = mypoint;
     //pid.point = pid.PIDval;//wheel1.getSumangle();
+    pid.dPoint = mypoint;
+    pid.dTarget = point;//wheel1.getSumangle();
+    float x = (float)pid.data;
     //float x = pid.PIDval;
     motor = x/5000;
+    //printf("%f %f %f \n",mypoint,point,x/5000);    
 }
-void SpeedStay();
-void SpeedStay_PID();
 
+void SpeedStay(float point , float mypoint ,int mode)
+{
+    float sa;
+    static float duty=0.1;
+    int X[2] = {Front , Back};
+    sa = (point-mypoint);
+    float a = sa;
+    if(sa<0)sa*=-1;
+    if(a>0.3)
+    {
+        motor.run(X[mode],sa*duty/1000);
+        duty+=1;
+    }
+    else if(a<-0.3)
+    {
+        motor.run(X[mode],sa*duty/1000);
+        duty-=1;
+    }
+    else motor.run(X[mode],sa*duty/1000);
+ 
+    //printf("%f %f %f \n",mypoint,point,sa*duty/1000);    
+}
+void SpeedStay_PID(float point , float mypoint,int mode)
+{
+    float x = 0;
+    //x = PIctrl(point , mypoint);
+    if(x<0)x*=-1;
+    //if(point<0)point*=-1;
+    int X[2] = {Front , Back};
+    pid.dPoint = mypoint;
+    pid.dTarget = point;//wheel1.getSumangle();
+    x = (float)pid.data/1000;
+    if(x<0.001&&x>-0.001)motor.run(Stop,x);
+    else               motor.run(X[mode],x);
+    //printf("%f %f %f %i\n",mypoint,point,x/1000,mode);    
+}
+char Inflag=0;
 void Motor_mode()
 {
+    static char a=0;
     char IF = Registar[MotorMode]&(~0x80);
     switch(IF)
     {
-        case 1:ManiacMotor_Mode(short(Registar[MotorPWM]<<8|Registar[MotorPWM2]),Registar[MotorState]);break;
-        case 2:AngleStay(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),wheel.getSumangle(),Registar[MotorMode]>>7);break;
-        case 3:AngleStay_PID(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),wheel.getSumangle());break;
-        //case 4:SpeedStay();break;
-        //case 5:SpeedStay_PID();break;
-        default:motor.run(0,Stop);
+        case 0:motor.run(Free,0);break;
+        case 1:ManiacMotor_Mode(float((int(Registar[MotorPWM])<<8)|Registar[MotorPWM2])/0xffff,Registar[MotorState]);break;
+        case 2:AngleStay(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),pulse*Registar[PulsePerAngle],Registar[MotorMode]>>7);break;
+        case 3:AngleStay_PID(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),pulse*Registar[PulsePerAngle],Registar[MotorMode]>>7);break;
+        case 4:SpeedStay(short(Registar[TargetSpeed]<<8|Registar[TargetSpeed2]),(float)PR*Registar[PulsePerAngle]/1000,int((Registar[TargetSpeed]>>7)==1));break;
+        case 5:SpeedStay_PID(short(Registar[TargetSpeed]<<8|Registar[TargetSpeed2]),(float)PR*Registar[PulsePerAngle]/1000,int((Registar[TargetSpeed]>>7)==1));break;
+        default:motor.run(Stop,0xffff);
     }
+    static bool bo=false;
+    if(!(IF==3||IF==5)&&bo==false)
+    {
+        //t.stop();
+        //printf("STOP\n");
+        pid.stop();
+        pid.s_dErrIntg = 0;
+        bo=true;
+    }
+    else if((IF==3||IF==5)&&bo==true)
+    {
+        bo=false;
+        //printf("START\n");
+        //t.stop();
+        pid.Start();
+    }
+    //printf("%d",IF);
+    if(IF!=a)
+        Led.Act((int)IF);
+    a=IF;
     //motor.run(Registar[MotorState],float(Registar[MotorPWM])/256.0);
     /*motor=float(Registar[MotorPWM]<<8|Registar[MotorPWM2]-32768)/32768.0;
     printf("%f\n",float(Registar[MotorPWM]<<8|Registar[MotorPWM2]-32768)/32768.0);*/
 }
 
-void encode(/*void const *argument*/)
+void Tic(/*void const *argument*/)
 {
-        //wheel.encode();
      Motor_mode();
      Rotate();
+     //printf("Tic\n");
 }
 //void timer(){
 extern "C"  void execute_spi_slave_hw( void )
 {
     //ledDbg = 1;
-    wheel.state(1);
+    //wheel.state(1);
     if(i2c->receive()==I2CSlave::WriteAddressed&&mode==I2C_MODE)
-        {
-            LED=Red;
+    {
+            //LED=Red;
             //encoder.stop();
             //wheel->state(1);
             char DATA[2] = {}; 
@@ -161,50 +299,54 @@
             char num =DATA[1]; 
             char X[num];
             char f=0;
-            //wait_us(1000);
-            printf("R registar %d ",reg);
+            wait_us(1000);
+            //printf("R registar %d \n",reg);
             switch(i2c->receive())
             {
                 case 0 :break;
                 case I2CSlave::ReadAddressed:
                 {
-                    char *po = Registar+reg;
+                    //char *po = Registar+reg;
                     for(int i=0;i<num;i++)
-                        X[i]=*po+i;
+                    {
+                        //char a=*po+i;
+                        X[i] = Registar[reg+i];
+                    }
                     i2c->write(X,num);
-                    f=1;
+                    /*f=1;
                     do
                     {
                         f = i2c->write(Registar[reg]);
                     //    printf(" %d ",Registar[reg]);
                         reg++;
-                    }while(f==1);
+                    }while(f==1);*/
                     break;
                 }
-                case I2CSlave::WriteGeneral:{break;}
+                case I2CSlave::WriteGeneral:{
+                    
+                    break;
+                }
                 case I2CSlave::WriteAddressed:
                 {
                     char num = DATA[1];
-                    for(int i=1; i<num; i++,reg++)
-                        char X[num];
                         i2c->read(X,num);
                         //Registar[reg]=D;
                         for (int i=0;i<num;i++)
                         {
                             Registar[reg]=X[i];
                          //   printf("%d ",Registar[reg]);
+                         //printf(" Registar : %d  ,%d\n",Registar[reg],reg);
                             reg++;
                         }
                         //printf(" Registar : %d  ,%d\n",Registar[reg],reg);
-                    
                     break;
                 }
             }
             //printf("OK\n");
-            wheel.state(0);
+            //wheel.state(0);
         }
         if(spi->receive()&&mode==SPI_MODE) {
-            LED=Blue|Red;
+            //LED=Blue|Red;
             //wheel.state(1);
             //rotateT.detach();
             //encoder.stop();
@@ -248,17 +390,27 @@
             
             //wheel.state(0);
             //encoder.start(10);
-        
+            wait(.01);
     }
-    wheel.state(0);
-
-
+    /*NVIC_ClearPendingIRQ (TIMER_16_0_IRQn);
+    NVIC_ClearPendingIRQ (TIMER_16_1_IRQn);
+    NVIC_ClearPendingIRQ (TIMER_32_0_IRQn);
+    NVIC_ClearPendingIRQ (TIMER_32_1_IRQn);*/
+    //wheel.state(0);
+    //encode();
+    Inflag=1;
+}
+extern "C" void HardFault_Handler() {
+    printf("Hard Fault!\n");
+    while(1);
 }
 int main() {
     //pc.baud(230400);
     Mode = new DigitalIn(MODE);
-    Registar[0x60]=10;
-    Registar[0x61]=32;
+    Registar[Who_am_I] = 0x67; 
+    Registar[MotorP] = GAIN_P;
+    Registar[MotorI] = GAIN_I;
+    Registar[MotorD] = GAIN_D;
     //rotateN.attach(&enc,0.001);
     //Motor=0x08|0x01;
     if(*Mode==1)
@@ -268,40 +420,41 @@
         spi->frequency(4000000);
         spi->reply(0x00);              // Prime SPI with first reply
         mode=SPI_MODE;
-        LED=Blue;
+        Led.Wait(0);
     }
     else if(*Mode==0)
     {
         i2c = new I2CSlave(SDA,SCL);
-        i2c->frequency(200000);
-        //char address[4]={0x20,0xb2,0xee,0xf4};
+        i2c->frequency(2000000);
+        char address[4]={0x02,0x04,0x06,0x08};
         Address = new BusIn(I2C_addr_L,I2C_addr_H);
         i2c->address(0xa0/*address[Address->read()]*/);
         mode=I2C_MODE;
-        LED=Green;
+        Led.Wait(1);
         delete Address ;
     }
     NVIC_SetVector( I2C_IRQn , ( uint32_t ) execute_spi_slave_hw ) ;
-    NVIC_SetPriority( I2C_IRQn , 20 ) ;
+    NVIC_SetPriority( I2C_IRQn , 1) ;
     NVIC_EnableIRQ( I2C_IRQn ) ;
     
-    NVIC_SetPriority(TIMER_16_0_IRQn,9);
-    NVIC_SetPriority(TIMER_16_1_IRQn,8);
-    NVIC_SetPriority(TIMER_32_0_IRQn,7);
+    NVIC_SetPriority(TIMER_16_0_IRQn,3);
+    NVIC_SetPriority(TIMER_16_1_IRQn,4);
+    NVIC_SetPriority(TIMER_32_0_IRQn,5);
     NVIC_SetPriority(TIMER_32_1_IRQn,6);
-    rotateT.attach(&encode,0.01);
-    
-    NVIC_SetPriority( EINT0_IRQn   , 5 ) ;
-    NVIC_SetPriority( EINT1_IRQn   , 4 ) ;
-    NVIC_SetPriority( EINT2_IRQn   , 3 ) ;
-    NVIC_SetPriority( EINT3_IRQn   , 2 ) ;
+    rotateT.attach(&Tic,0.005);
     while(1) {
-    
-        /*Motor_mode();   
-        Rotate();
-        encode();*/
+        
+    //motor.run(1,1);
+    //    Motor_mode();   
+    //    Rotate();
+        /*while(10-ti.read_ms()>0)
+        {}
+            ti.reset();
+            encode();*/
+        
     }
 }
  
 
 
+