Diff: main.cpp

Revision:

2:1194c29429bf

Parent:

0:5af71b978fb7

Child:

3:cc301e19f4d1

--- 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();*/
+        
     }
 }
  
 
 
+