Kiko Ishimoto
ＭＢＥＤテスト
Diff: main.cpp
- Revision:
- 0:5af71b978fb7
- Child:
- 2:1194c29429bf
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Jun 19 06:36:41 2015 +0000
@@ -0,0 +1,307 @@
+// Reply to a SPI master as slave
+float GAIN_P = 4.5f; // 比例ｹﾞｲﾝ
+float GAIN_I = 3.7f; // 積分ｹﾞｲﾝ
+float GAIN_D = 1.0f;
+
+#include "mbed.h"
+#include "Motor.h"
+#include "QEI.h"
+#include "PID.h"
+#include "Defines.h"
+
+char Registar[0x80]={123};
+char mode;
+Timer timer;
+double PIctrl(double dCommand, double dVal)
+ {
+ static double s_dErrIntg = 0 ,dErr_prev=0;
+ double dErr;
+ double dRet;
+
+ // 誤差
+dErr = dCommand - dVal;
+
+ // 誤差積分
+s_dErrIntg += (dErr+dErr_prev )* timer.read() /2.0;
+
+ // 制御入力
+dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*(dErr-dErr_prev)/timer.read();
+timer.reset();
+dErr_prev = dErr;
+ return (dRet);
+}
+PinName rotatepin[3];
+Ticker rotateT;
+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);
+
+void ManiacMotor_Mode(short duty,char mode)
+{
+ motor.run(mode,duty);
+ //motor=1;
+}
+//PinName rotatepin[3];
+int rotatemode;
+void RotateSet()
+{
+ //delete wheel;
+ short 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;
+ //else rotatepin[0]=dp13;
+ if(Registar[RotateMode]&0x04)rotatepin[2]=Rotal_Z;
+ //else rotatepin[0]=dp13;*/
+ if(Registar[RotateMode]&0x08)rotatemode=1;
+ 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");
+ }
+}
+void Rotate(/*void const *argument*/)
+{
+ static bool flag=0;
+ if(Registar[RotateMode]&0x80)
+ {
+ if(!flag)
+ {
+ RotateSet();
+ flag=1;
+ }
+ short pulse = short(wheel.getPulses());
+ short Rev = short(wheel.getRevolutions());
+ short PR = short(wheel.getRPUS()*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]);
+ }
+ /*if(flag==1)
+ {
+ static char Reg;
+ static short pul;
+ if(Registar[RotateMode]!=Reg||short(Registar[PulsePerRev]<<8|Registar[PulsePerRev2])!=pul)
+ flag=0;
+ }*/
+}
+void AngleStay(float point , float mypoint ,bool mode)
+{
+ float sa;
+ if(mode==1) sa = (int)point%360-(int)mypoint%360;
+ else sa = point-mypoint;
+ if(sa>10)
+ motor.run(Front,1);
+ else if(sa<-10)
+ motor.run(Back,1);
+ else motor.run(Stop,1);
+
+ printf("%f %f %f \n",mypoint,point,sa);
+}
+void AngleStay_PID(float point , float mypoint)
+{
+ float x = PIctrl(point , mypoint);
+ //pid.dPoint = mypoint;
+ //pid.point = pid.PIDval;//wheel1.getSumangle();
+ //float x = pid.PIDval;
+ motor = x/5000;
+}
+void SpeedStay();
+void SpeedStay_PID();
+
+void Motor_mode()
+{
+ 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);
+ }
+ //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*/)
+{
+ //wheel.encode();
+ Motor_mode();
+ Rotate();
+}
+//void timer(){
+extern "C" void execute_spi_slave_hw( void )
+{
+ //ledDbg = 1;
+ wheel.state(1);
+ if(i2c->receive()==I2CSlave::WriteAddressed&&mode==I2C_MODE)
+ {
+ LED=Red;
+ //encoder.stop();
+ //wheel->state(1);
+ char DATA[2] = {};
+ i2c->read(DATA,2);
+ char reg=DATA[0];
+ char num =DATA[1];
+ char X[num];
+ char f=0;
+ //wait_us(1000);
+ printf("R registar %d ",reg);
+ switch(i2c->receive())
+ {
+ case 0 :break;
+ case I2CSlave::ReadAddressed:
+ {
+ char *po = Registar+reg;
+ for(int i=0;i<num;i++)
+ X[i]=*po+i;
+ i2c->write(X,num);
+ f=1;
+ do
+ {
+ f = i2c->write(Registar[reg]);
+ // printf(" %d ",Registar[reg]);
+ reg++;
+ }while(f==1);
+ 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]);
+ reg++;
+ }
+ //printf(" Registar : %d ,%d\n",Registar[reg],reg);
+
+ break;
+ }
+ }
+ //printf("OK\n");
+ wheel.state(0);
+ }
+ if(spi->receive()&&mode==SPI_MODE) {
+ LED=Blue|Red;
+ //wheel.state(1);
+ //rotateT.detach();
+ //encoder.stop();
+ char flag=1;
+ char reg = spi->read();
+ wait_us(50);
+ char num = spi->read();
+ //printf("SIZE %d\n",num);
+ if(reg&0x80){
+ reg=reg&(~0x80);
+ flag=0;
+ spi->reply(Registar[reg]);
+ }
+ else spi->reply(0x00);
+ //wait_us(10);
+
+ if(flag)
+ for(int i=0;i<num;i++)
+ {
+ while(!spi->receive());
+ wait_us(50);
+ Registar[reg+i] = spi->read();
+ //printf("%d,%d\n",reg+i,Registar[reg+i]);
+ }
+ else
+ {
+ for(int i=0;i<num;i++)
+ {
+ //
+ while(!spi->receive());
+ //wait_us(50);
+ spi->reply(Registar[reg++]);
+ char dummy = spi->read();
+ //printf("%d,%d\n",reg+i,Registar[reg+i]);
+ }
+
+ }
+ //printf("%d , %d\n",reg,Registar[reg]);
+ flag=1;
+ //spi->reply(00);
+
+ //wheel.state(0);
+ //encoder.start(10);
+
+ }
+ wheel.state(0);
+
+
+}
+int main() {
+ //pc.baud(230400);
+ Mode = new DigitalIn(MODE);
+ Registar[0x60]=10;
+ Registar[0x61]=32;
+ //rotateN.attach(&enc,0.001);
+ //Motor=0x08|0x01;
+ if(*Mode==1)
+ {
+ spi = new SPISlave(MOSI, MISO, SCK,SSEL);
+ spi->format(8,1);
+ spi->frequency(4000000);
+ spi->reply(0x00); // Prime SPI with first reply
+ mode=SPI_MODE;
+ LED=Blue;
+ }
+ else if(*Mode==0)
+ {
+ i2c = new I2CSlave(SDA,SCL);
+ i2c->frequency(200000);
+ //char address[4]={0x20,0xb2,0xee,0xf4};
+ Address = new BusIn(I2C_addr_L,I2C_addr_H);
+ i2c->address(0xa0/*address[Address->read()]*/);
+ mode=I2C_MODE;
+ LED=Green;
+ delete Address ;
+ }
+ NVIC_SetVector( I2C_IRQn , ( uint32_t ) execute_spi_slave_hw ) ;
+ NVIC_SetPriority( I2C_IRQn , 20 ) ;
+ 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_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 ) ;
+ while(1) {
+
+ /*Motor_mode();
+ Rotate();
+ encode();*/
+ }
+}
+
+
+