Diff: main.cpp

Revision:

2:897a1cac8d96

Parent:

0:5af71b978fb7

--- a/main.cpp	Fri Jun 19 06:38:52 2015 +0000
+++ b/main.cpp	Thu Sep 03 01:28:32 2015 +0000
@@ -1,159 +1,26 @@
-// 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};
+BusOut tinko(dp28,dp26,dp24,dp18,dp14,dp13);
+     //bit    1    2    3    4    5    6    7    8
+char Registar[0x80]={123}; //128ko 123 ha kinisuruna
 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*/)
+void action(/*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;
-    }*/
+    tinko = short(Registar[Canon]);
 }
-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);
@@ -200,8 +67,6 @@
                     break;
                 }
             }
-            //printf("OK\n");
-            wheel.state(0);
         }
         if(spi->receive()&&mode==SPI_MODE) {
             LED=Blue|Red;
@@ -250,17 +115,13 @@
             //encoder.start(10);
         
     }
-    wheel.state(0);
-
 
 }
 int main() {
     //pc.baud(230400);
+    LED=0;
+    Registar[Who_am_I] = 0x10;
     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);
@@ -289,19 +150,25 @@
     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);
+    rotateT.attach(&action,0.01);
     
     NVIC_SetPriority( EINT0_IRQn   , 5 ) ;
     NVIC_SetPriority( EINT1_IRQn   , 4 ) ;
     NVIC_SetPriority( EINT2_IRQn   , 3 ) ;
     NVIC_SetPriority( EINT3_IRQn   , 2 ) ;
+    tinko = 0x0000;
     while(1) {
-    
-        /*Motor_mode();   
-        Rotate();
-        encode();*/
+            if(i2c->receive()==I2CSlave::WriteAddressed&&mode==I2C_MODE)
+            {
+                LED=Red;
+            }
+            else if(mode==I2C_MODE)
+            {
+                LED=Green;
+            }
+            else
+            {
+                LED=Blue;
+            }
     }
-}
- 
-
-
+}
\ No newline at end of file