hidaka sato
anal
Fork of
2015_denziben_i2c_S
by 
hidaka sato
Diff: main.cpp
- Revision:
- 3:1194c29429bf
- Parent:
- 0:5af71b978fb7
--- 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();*/
+
}
}
+