main.cpp

00001 // Reply to a SPI master as slave
00002 float GAIN_P = 4.3f; // 比例ｹﾞｲﾝ
00003 float GAIN_I = 1.3f; // 積分ｹﾞｲﾝ
00004 float GAIN_D = 1.21f;
00005  
00006 #include "mbed.h"
00007 #include "Motor.h"
00008 #include "QEI.h"
00009 #include "PID.h"
00010 #include "Defines.h"
00011 #include "Registar.h"
00012 BusOut LED(Red_Pin,Green_Pin,Blue_Pin);
00013 class LEDMode
00014 {
00015     Ticker Tic;
00016     private:
00017         char led,Off;
00018         void Inter()
00019         {
00020             static int flag;
00021             flag++;
00022             if(flag&2)
00023             {
00024                 LED = led;
00025             }
00026             else
00027             {
00028                 Tic.attach(this,&LEDMode::Inter,TransWait*0.3);
00029                 LED = Off; 
00030             } 
00031         }
00032     public:
00033         char Red,Blue,Green;
00034         char TransWait;
00035         char InterVal;
00036         char Mode[7];// = {RED|BLUE,RED|GREEN,RED|GREEN,BLUE|GREEN,BLUE|GREEN};
00037         float Int[7];
00038         char OFF[7];
00039         LEDMode()
00040         {
00041             char mode[7] = {RED,RED|BLUE,RED|GREEN,RED|GREEN,BLUE|GREEN,BLUE|GREEN,RED|BLUE};
00042             char off[7] = {0,0,0,GREEN,0,GREEN,RED};
00043             float in[7] = {1,1,1.5,1.7,1.5,1.7,1.0};
00044             for(int i=0;i<7;i++)
00045             {
00046                 Mode[i]=mode[i];
00047                 Int[i]=in[i];
00048                 OFF[i]=off[i];
00049             }
00050             Red=RED;
00051             Blue=BLUE;
00052             Green=GREEN;
00053             Off=0;
00054         }
00055         void Wait(int i)
00056         {
00057             float in = 1.0;
00058             TransWait = in;
00059             if(i==0)led = BLUE;
00060             else led = GREEN;
00061             Off=0;
00062             Tic.detach();
00063             Tic.attach(this,&LEDMode::Inter,TransWait);
00064         }
00065         void Act(int i)
00066         {
00067             if(i>=6)i=6; 
00068             {
00069                 Off=OFF[i];
00070                 led = Mode[i];
00071                 TransWait = Int[i];
00072                 Tic.detach();
00073                 Tic.attach(this,&LEDMode::Inter,TransWait);
00074             }
00075         }
00076 };
00077 LEDMode Led;
00078 PID pid(GAIN_P,GAIN_I,GAIN_D);
00079 short PR;
00080 short pulse ;
00081 short Rev;
00082 char Registar[0x80]={0};
00083 char mode;
00084 /*double PIctrl(double dCommand, double dVal)
00085  {
00086  static double s_dErrIntg = 0 ,dErr_prev=0;
00087  double dErr;
00088  double dRet;
00089 
00090  // 誤差
00091 dErr = dCommand - dVal;
00092 
00093  // 誤差積分
00094 s_dErrIntg += (dErr+dErr_prev )* timer.read() /2.0;
00095 
00096  // 制御入力
00097 dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*(dErr-dErr_prev)/timer.read();
00098 //printf("%f    ",timer.read());
00099 timer.reset();
00100 //printf("%f    ",dRet);
00101 dErr_prev = dErr;
00102  return (dRet);
00103 }*/
00104 Ticker rotateT;
00105 //Ticker rotateN;
00106 //QEI wheel(Rotal_A,Rotal_B,NC,100,QEI::X4_ENCODING);
00107 
00108     
00109 Motor motor(Motor_H1,Motor_L2,Motor_L1,Motor_H2,Motor_PWM,short(0xffff));
00110 
00111 void ManiacMotor_Mode(float duty,char mode)
00112 {
00113     motor.run(mode,duty);
00114     //motor=1;
00115     //printf("%f %f\n " , duty,1.0/duty);
00116 }
00117 //PinName rotatepin[3];
00118 int rotatemode;
00119 void RotateSet()
00120 {
00121     //delete wheel;
00122     //pulse = short(Registar[PulsePerRev]<<8|Registar[PulsePerRev2]);
00123     /*if(Registar[RotateMode]&0x01)rotatepin[0]=Rotal_A;
00124     //else rotatepin[0]=dp13;
00125     if(Registar[RotateMode]&0x02)rotatepin[1]=Rotal_B;
00126     //else rotatepin[0]=dp13;
00127     if(Registar[RotateMode]&0x04)rotatepin[2]=Rotal_Z;
00128     //else rotatepin[0]=dp13;*/
00129     if(Registar[RotateMode]&0x08)rotatemode=1;
00130     else if(!Registar[RotateMode]&0x08)rotatemode=0;
00131     if(Registar[RotateMode]&0x80)
00132     { 
00133         //wheel.SetUp(rotatepin[0],rotatepin[1],rotatepin[2],pulse,rotatemode);
00134         //printf("make QEI\n");
00135     }
00136 }
00137 void Rotate(/*void const *argument*/)
00138 {
00139     static bool flag=0;
00140     
00141     if(/*Registar[RotateMode]&0x80*/1)
00142     {
00143         if(!flag)
00144         {
00145             RotateSet();
00146             flag=1;
00147         }
00148         pulse = short((Registar[PulsePerRev]<<8)|Registar[PulsePerRev+1]/*wheel.getPulses()*/);
00149         Rev = short((Registar[PulsePerRev]<<8)|Registar[PulsePerRev]/*wheel.getRevolutions()*/);
00150         PR = short((Registar[PulsePerSec]<<8)|Registar[PulsePerSec+1]/*wheel.getRPMS()*1000*/);
00151         Registar[MoterRevolutionH] = (Rev&0xff00)>>8;
00152         Registar[MoterRevolutionH-1] = (Rev&0xff);
00153         Registar[MoterPulseH] = (pulse&0xff00)>>8;
00154         Registar[MoterPulseH-1] = (pulse&0xff);
00155         Registar[MoterSpeedH] = (PR&0xff00)>>8;
00156         Registar[MoterSpeedH-1] = (PR&0xff);
00157         char IF = Registar[MotorMode]&(~0x80);
00158         if(IF == 3 || IF == 5)
00159         {
00160             pid.GAIN_P = (float)Registar[MotorP]/10;
00161             pid.GAIN_I = (float)Registar[MotorI]/10;
00162             pid.GAIN_D = (float)Registar[MotorD]/10;
00163             //printf("PID P%f I%f D%f \n",pid.GAIN_P,pid.GAIN_I,pid.GAIN_D);
00164         }
00165         //printf("Rotate::: %d ,%d ,%d\n",pulse, Registar[PulsePerRev], Registar[PulsePerRev+1]);
00166     }
00167     /*if(flag==1)
00168     {
00169         static char Reg;
00170         static short pul;
00171         if(Registar[RotateMode]!=Reg||short(Registar[PulsePerRev]<<8|Registar[PulsePerRev2])!=pul)
00172         flag=0;
00173     }*/
00174 }
00175 void AngleStay(float point , float mypoint ,bool mode)
00176 {
00177     float sa;
00178     if(mode==1)    sa = (int)point%360-(int)mypoint%360;
00179     else        sa = point-mypoint;
00180     if(sa>10)
00181         motor.run(Front,1);
00182     else if(sa<-10)
00183         motor.run(Back,1);
00184     else motor.run(Stop,1);
00185  
00186     //printf("%f %f %f \n",mypoint,point,sa);    
00187 }
00188 void AngleStay_PID(float point , float mypoint,bool mode)
00189 {
00190     //float x = PIctrl(point , mypoint);
00191     float sa=0;
00192     if(mode==1)    sa = (int)point%360-(int)mypoint%360;
00193     else        sa = point-mypoint;
00194     //pid.dPoint = mypoint;
00195     //pid.point = pid.PIDval;//wheel1.getSumangle();
00196     pid.dPoint = mypoint;
00197     pid.dTarget = point;//wheel1.getSumangle();
00198     float x = (float)pid.data;
00199     //float x = pid.PIDval;
00200     motor = x/5000;
00201     //printf("%f %f %f \n",mypoint,point,x/5000);    
00202 }
00203 
00204 void SpeedStay(float point , float mypoint ,int mode)
00205 {
00206     float sa;
00207     static float duty=0.1;
00208     int X[2] = {Front , Back};
00209     sa = (point-mypoint);
00210     float a = sa;
00211     if(sa<0)sa*=-1;
00212     if(a>0.3)
00213     {
00214         motor.run(X[mode],sa*duty/1000);
00215         duty+=1;
00216     }
00217     else if(a<-0.3)
00218     {
00219         motor.run(X[mode],sa*duty/1000);
00220         duty-=1;
00221     }
00222     else motor.run(X[mode],sa*duty/1000);
00223  
00224     //printf("%f %f %f \n",mypoint,point,sa*duty/1000);    
00225 }
00226 void SpeedStay_PID(float point , float mypoint,int mode)
00227 {
00228     float x = 0;
00229     //x = PIctrl(point , mypoint);
00230     if(x<0)x*=-1;
00231     //if(point<0)point*=-1;
00232     int X[2] = {Front , Back};
00233     pid.dPoint = mypoint;
00234     pid.dTarget = point;//wheel1.getSumangle();
00235     x = (float)pid.data/1000;
00236     if(x<0.001&&x>-0.001)motor.run(Stop,x);
00237     else               motor.run(X[mode],x);
00238     //printf("%f %f %f %i\n",mypoint,point,x/1000,mode);    
00239 }
00240 char Inflag=0;
00241 void Motor_mode()
00242 {
00243     static char a=0;
00244     char IF = Registar[MotorMode]&(~0x80);
00245     switch(IF)
00246     {
00247         case 0:motor.run(Free,0);break;
00248         case 1:ManiacMotor_Mode(float((int(Registar[MotorPWM])<<8)|Registar[MotorPWM2])/0xffff,Registar[MotorState]);break;
00249         case 2:AngleStay(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),pulse*Registar[PulsePerAngle],Registar[MotorMode]>>7);break;
00250         case 3:AngleStay_PID(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),pulse*Registar[PulsePerAngle],Registar[MotorMode]>>7);break;
00251         case 4:SpeedStay(short(Registar[TargetSpeed]<<8|Registar[TargetSpeed2]),(float)PR*Registar[PulsePerAngle]/1000,int((Registar[TargetSpeed]>>7)==1));break;
00252         case 5:SpeedStay_PID(short(Registar[TargetSpeed]<<8|Registar[TargetSpeed2]),(float)PR*Registar[PulsePerAngle]/1000,int((Registar[TargetSpeed]>>7)==1));break;
00253         default:motor.run(Stop,0xffff);
00254     }
00255     static bool bo=false;
00256     if(!(IF==3||IF==5)&&bo==false)
00257     {
00258         //t.stop();
00259         //printf("STOP\n");
00260         pid.stop();
00261         pid.s_dErrIntg = 0;
00262         bo=true;
00263     }
00264     else if((IF==3||IF==5)&&bo==true)
00265     {
00266         bo=false;
00267         //printf("START\n");
00268         //t.stop();
00269         pid.Start();
00270     }
00271     //printf("%d",IF);
00272     if(IF!=a)
00273         Led.Act((int)IF);
00274     a=IF;
00275     //motor.run(Registar[MotorState],float(Registar[MotorPWM])/256.0);
00276     /*motor=float(Registar[MotorPWM]<<8|Registar[MotorPWM2]-32768)/32768.0;
00277     printf("%f\n",float(Registar[MotorPWM]<<8|Registar[MotorPWM2]-32768)/32768.0);*/
00278 }
00279 
00280 void Tic(/*void const *argument*/)
00281 {
00282      Motor_mode();
00283      Rotate();
00284      //printf("Tic\n");
00285 }
00286 //void timer(){
00287 extern "C"  void execute_spi_slave_hw( void )
00288 {
00289     //ledDbg = 1;
00290     //wheel.state(1);
00291     if(i2c->receive()==I2CSlave::WriteAddressed&&mode==I2C_MODE)
00292     {
00293             //LED=Red;
00294             //encoder.stop();
00295             //wheel->state(1);
00296             char DATA[2] = {}; 
00297             i2c->read(DATA,2);
00298             char reg=DATA[0];
00299             char num =DATA[1]; 
00300             char X[num];
00301             char f=0;
00302             wait_us(1000);
00303             //printf("R registar %d \n",reg);
00304             switch(i2c->receive())
00305             {
00306                 case 0 :break;
00307                 case I2CSlave::ReadAddressed:
00308                 {
00309                     //char *po = Registar+reg;
00310                     for(int i=0;i<num;i++)
00311                     {
00312                         //char a=*po+i;
00313                         X[i] = Registar[reg+i];
00314                     }
00315                     i2c->write(X,num);
00316                     /*f=1;
00317                     do
00318                     {
00319                         f = i2c->write(Registar[reg]);
00320                     //    printf(" %d ",Registar[reg]);
00321                         reg++;
00322                     }while(f==1);*/
00323                     break;
00324                 }
00325                 case I2CSlave::WriteGeneral:{
00326                     
00327                     break;
00328                 }
00329                 case I2CSlave::WriteAddressed:
00330                 {
00331                     char num = DATA[1];
00332                         i2c->read(X,num);
00333                         //Registar[reg]=D;
00334                         for (int i=0;i<num;i++)
00335                         {
00336                             Registar[reg]=X[i];
00337                          //   printf("%d ",Registar[reg]);
00338                          //printf(" Registar : %d  ,%d\n",Registar[reg],reg);
00339                             reg++;
00340                         }
00341                         //printf(" Registar : %d  ,%d\n",Registar[reg],reg);
00342                     break;
00343                 }
00344             }
00345             //printf("OK\n");
00346             //wheel.state(0);
00347         }
00348         if(spi->receive()&&mode==SPI_MODE) {
00349             //LED=Blue|Red;
00350             //wheel.state(1);
00351             //rotateT.detach();
00352             //encoder.stop();
00353             char flag=1;
00354             char reg = spi->read();
00355             wait_us(50);
00356             char num = spi->read();
00357             //printf("SIZE %d\n",num);
00358             if(reg&0x80){
00359                 reg=reg&(~0x80);
00360                 flag=0;
00361                 spi->reply(Registar[reg]);
00362             }
00363             else spi->reply(0x00);
00364             //wait_us(10);
00365             
00366             if(flag)
00367                 for(int i=0;i<num;i++)
00368                 {
00369                     while(!spi->receive());
00370                     wait_us(50);
00371                     Registar[reg+i] = spi->read();
00372                     //printf("%d,%d\n",reg+i,Registar[reg+i]);
00373                 }
00374             else 
00375             {
00376                 for(int i=0;i<num;i++)
00377                 {
00378                     //
00379                     while(!spi->receive());
00380                     //wait_us(50);
00381                     spi->reply(Registar[reg++]);
00382                     char dummy = spi->read();
00383                     //printf("%d,%d\n",reg+i,Registar[reg+i]);
00384                 }
00385                 
00386             }
00387             //printf("%d  , %d\n",reg,Registar[reg]);
00388             flag=1;
00389             //spi->reply(00);
00390             
00391             //wheel.state(0);
00392             //encoder.start(10);
00393             wait(.01);
00394     }
00395     /*NVIC_ClearPendingIRQ (TIMER_16_0_IRQn);
00396     NVIC_ClearPendingIRQ (TIMER_16_1_IRQn);
00397     NVIC_ClearPendingIRQ (TIMER_32_0_IRQn);
00398     NVIC_ClearPendingIRQ (TIMER_32_1_IRQn);*/
00399     //wheel.state(0);
00400     //encode();
00401     Inflag=1;
00402 }
00403 extern "C" void HardFault_Handler() {
00404     printf("Hard Fault!\n");
00405     while(1);
00406 }
00407 int main() {
00408     //pc.baud(230400);
00409     Mode = new DigitalIn(MODE);
00410     Registar[Who_am_I] = 0x67; 
00411     Registar[MotorP] = GAIN_P;
00412     Registar[MotorI] = GAIN_I;
00413     Registar[MotorD] = GAIN_D;
00414     //rotateN.attach(&enc,0.001);
00415     //Motor=0x08|0x01;
00416     if(*Mode==1)
00417     {
00418         spi = new SPISlave(MOSI, MISO, SCK,SSEL);
00419         spi->format(8,1);
00420         spi->frequency(4000000);
00421         spi->reply(0x00);              // Prime SPI with first reply
00422         mode=SPI_MODE;
00423         Led.Wait(0);
00424     }
00425     else if(*Mode==0)
00426     {
00427         i2c = new I2CSlave(SDA,SCL);
00428         i2c->frequency(2000000);
00429         char address[4]={0x02,0x04,0x06,0x08};
00430         Address = new BusIn(I2C_addr_L,I2C_addr_H);
00431         i2c->address(0xa0/*address[Address->read()]*/);
00432         mode=I2C_MODE;
00433         Led.Wait(1);
00434         delete Address ;
00435     }
00436     NVIC_SetVector( I2C_IRQn , ( uint32_t ) execute_spi_slave_hw ) ;
00437     NVIC_SetPriority( I2C_IRQn , 1) ;
00438     NVIC_EnableIRQ( I2C_IRQn ) ;
00439     
00440     NVIC_SetPriority(TIMER_16_0_IRQn,3);
00441     NVIC_SetPriority(TIMER_16_1_IRQn,4);
00442     NVIC_SetPriority(TIMER_32_0_IRQn,5);
00443     NVIC_SetPriority(TIMER_32_1_IRQn,6);
00444     rotateT.attach(&Tic,0.005);
00445     while(1) {
00446         
00447     //motor.run(1,1);
00448     //    Motor_mode();   
00449     //    Rotate();
00450         /*while(10-ti.read_ms()>0)
00451         {}
00452             ti.reset();
00453             encode();*/
00454         
00455     }
00456 }
00457  
00458 
00459 
00460