Revision 0:5af71b978fb7, committed 2015-06-19

Comitter:

kikoaac

Date:

Fri Jun 19 06:36:41 2015 +0000

Child:

1:6f51a981ed09

Commit message:

??????????

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Defines.h	Fri Jun 19 06:36:41 2015 +0000
@@ -0,0 +1,24 @@
+#define Red_Pin dp9
+#define Green_Pin dp16
+#define Blue_Pin dp17
+#define MOSI dp2
+#define MISO dp1
+#define SCK  dp6
+#define SSEL dp25
+#define SDA  dp5
+#define SCL  dp27
+#define Motor_H1 dp24
+#define Motor_H2 dp4
+#define Motor_L1 dp28
+#define Motor_L2 dp26
+#define Motor_PWM dp18
+#define Rotal_A dp14
+#define Rotal_B dp11
+#define Rotal_Z dp15
+#define I2C_addr_H dp25
+#define I2C_addr_L dp6
+#define MODE dp15
+
+
+#define SPI_MODE 1
+#define I2C_MODE 2
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Fri Jun 19 06:36:41 2015 +0000
@@ -0,0 +1,1 @@
+Motor#1252cd3b995f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Fri Jun 19 06:36:41 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/PID/#aac6180820a4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Fri Jun 19 06:36:41 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/kikoaac/code/QEI/#1f4e8614d0ed

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Registar.h	Fri Jun 19 06:36:41 2015 +0000
@@ -0,0 +1,40 @@
+#include "mbed.h"
+#include "Defines.h"
+#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 MoterRevolutionL   0x11
+#define MoterRevolutionH   0x12
+#define MoterPulseL 0x13
+#define MoterPulseH 0x14
+#define MoterSpeedL 0x15
+#define MoterSpeedH 0x16
+#define Automatic   0x0
+#define MotorP 0x23
+#define MotorI 0x24
+#define MotorD 0x25
+#define FinishAuto 0x00
+#define TargetSpeed 0x00
+#define TargetAngle2 0x22
+#define TargetAngle 0x21
+#define TargetSpeed2 0x00
+#define PulsePerRev 0x18
+#define PulsePerRev2 0x19
+#define RotateMode  0x17
+
+
+

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Jun 19 06:36:41 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/8ab26030e058
\ No newline at end of file