Kiko Ishimoto / Mbed 2 deprecated Nucleo_spi

ＭＢＥＤテスト

Dependencies:   Motor PID mbed

main.cpp@0:5af71b978fb7, 2015-06-19 (annotated)

Committer:
kikoaac
Date:
Fri Jun 19 06:36:41 2015 +0000
Revision:
0:5af71b978fb7
Child:
2:1194c29429bf
??????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kikoaac 0:5af71b978fb7 1 // Reply to a SPI master as slave
kikoaac 0:5af71b978fb7 2 float GAIN_P = 4.5f; // 比例ｹﾞｲﾝ
kikoaac 0:5af71b978fb7 3 float GAIN_I = 3.7f; // 積分ｹﾞｲﾝ
kikoaac 0:5af71b978fb7 4 float GAIN_D = 1.0f;
kikoaac 0:5af71b978fb7 5
kikoaac 0:5af71b978fb7 6 #include "mbed.h"
kikoaac 0:5af71b978fb7 7 #include "Motor.h"
kikoaac 0:5af71b978fb7 8 #include "QEI.h"
kikoaac 0:5af71b978fb7 9 #include "PID.h"
kikoaac 0:5af71b978fb7 10 #include "Defines.h"
kikoaac 0:5af71b978fb7 11
kikoaac 0:5af71b978fb7 12 char Registar[0x80]={123};
kikoaac 0:5af71b978fb7 13 char mode;
kikoaac 0:5af71b978fb7 14 Timer timer;
kikoaac 0:5af71b978fb7 15 double PIctrl(double dCommand, double dVal)
kikoaac 0:5af71b978fb7 16 {
kikoaac 0:5af71b978fb7 17 static double s_dErrIntg = 0 ,dErr_prev=0;
kikoaac 0:5af71b978fb7 18 double dErr;
kikoaac 0:5af71b978fb7 19 double dRet;
kikoaac 0:5af71b978fb7 20
kikoaac 0:5af71b978fb7 21 // 誤差
kikoaac 0:5af71b978fb7 22 dErr = dCommand - dVal;
kikoaac 0:5af71b978fb7 23
kikoaac 0:5af71b978fb7 24 // 誤差積分
kikoaac 0:5af71b978fb7 25 s_dErrIntg += (dErr+dErr_prev )* timer.read() /2.0;
kikoaac 0:5af71b978fb7 26
kikoaac 0:5af71b978fb7 27 // 制御入力
kikoaac 0:5af71b978fb7 28 dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*(dErr-dErr_prev)/timer.read();
kikoaac 0:5af71b978fb7 29 timer.reset();
kikoaac 0:5af71b978fb7 30 dErr_prev = dErr;
kikoaac 0:5af71b978fb7 31 return (dRet);
kikoaac 0:5af71b978fb7 32 }
kikoaac 0:5af71b978fb7 33 PinName rotatepin[3];
kikoaac 0:5af71b978fb7 34 Ticker rotateT;
kikoaac 0:5af71b978fb7 35 Ticker rotateN;
kikoaac 0:5af71b978fb7 36 QEI wheel(Rotal_A,Rotal_B,NC,100,QEI::X4_ENCODING);
kikoaac 0:5af71b978fb7 37
kikoaac 0:5af71b978fb7 38
kikoaac 0:5af71b978fb7 39 #include "Registar.h"
kikoaac 0:5af71b978fb7 40 Motor motor(Motor_H1,Motor_L2,Motor_L1,Motor_H2,Motor_PWM,1);
kikoaac 0:5af71b978fb7 41
kikoaac 0:5af71b978fb7 42 void ManiacMotor_Mode(short duty,char mode)
kikoaac 0:5af71b978fb7 43 {
kikoaac 0:5af71b978fb7 44 motor.run(mode,duty);
kikoaac 0:5af71b978fb7 45 //motor=1;
kikoaac 0:5af71b978fb7 46 }
kikoaac 0:5af71b978fb7 47 //PinName rotatepin[3];
kikoaac 0:5af71b978fb7 48 int rotatemode;
kikoaac 0:5af71b978fb7 49 void RotateSet()
kikoaac 0:5af71b978fb7 50 {
kikoaac 0:5af71b978fb7 51 //delete wheel;
kikoaac 0:5af71b978fb7 52 short pulse = short(Registar[PulsePerRev]<<8|Registar[PulsePerRev2]);
kikoaac 0:5af71b978fb7 53 /*if(Registar[RotateMode]&0x01)rotatepin[0]=Rotal_A;
kikoaac 0:5af71b978fb7 54 //else rotatepin[0]=dp13;
kikoaac 0:5af71b978fb7 55 if(Registar[RotateMode]&0x02)rotatepin[1]=Rotal_B;
kikoaac 0:5af71b978fb7 56 //else rotatepin[0]=dp13;
kikoaac 0:5af71b978fb7 57 if(Registar[RotateMode]&0x04)rotatepin[2]=Rotal_Z;
kikoaac 0:5af71b978fb7 58 //else rotatepin[0]=dp13;*/
kikoaac 0:5af71b978fb7 59 if(Registar[RotateMode]&0x08)rotatemode=1;
kikoaac 0:5af71b978fb7 60 else if(!Registar[RotateMode]&0x08)rotatemode=0;
kikoaac 0:5af71b978fb7 61 if(Registar[RotateMode]&0x80)
kikoaac 0:5af71b978fb7 62 {
kikoaac 0:5af71b978fb7 63 timer.start();
kikoaac 0:5af71b978fb7 64 //wheel.SetUp(rotatepin[0],rotatepin[1],rotatepin[2],pulse,rotatemode);
kikoaac 0:5af71b978fb7 65 //printf("make QEI\n");
kikoaac 0:5af71b978fb7 66 }
kikoaac 0:5af71b978fb7 67 }
kikoaac 0:5af71b978fb7 68 void Rotate(/*void const *argument*/)
kikoaac 0:5af71b978fb7 69 {
kikoaac 0:5af71b978fb7 70 static bool flag=0;
kikoaac 0:5af71b978fb7 71 if(Registar[RotateMode]&0x80)
kikoaac 0:5af71b978fb7 72 {
kikoaac 0:5af71b978fb7 73 if(!flag)
kikoaac 0:5af71b978fb7 74 {
kikoaac 0:5af71b978fb7 75 RotateSet();
kikoaac 0:5af71b978fb7 76 flag=1;
kikoaac 0:5af71b978fb7 77 }
kikoaac 0:5af71b978fb7 78 short pulse = short(wheel.getPulses());
kikoaac 0:5af71b978fb7 79 short Rev = short(wheel.getRevolutions());
kikoaac 0:5af71b978fb7 80 short PR = short(wheel.getRPUS()*1000);
kikoaac 0:5af71b978fb7 81 Registar[MoterRevolutionH] = (Rev&0xff00)>>8;
kikoaac 0:5af71b978fb7 82 Registar[MoterRevolutionH-1] = (Rev&0xff);
kikoaac 0:5af71b978fb7 83 Registar[MoterPulseH] = (pulse&0xff00)>>8;
kikoaac 0:5af71b978fb7 84 Registar[MoterPulseH-1] = (pulse&0xff);
kikoaac 0:5af71b978fb7 85 Registar[MoterSpeedH] = (PR&0xff00)>>8;
kikoaac 0:5af71b978fb7 86 Registar[MoterSpeedH-1] = (PR&0xff);
kikoaac 0:5af71b978fb7 87
kikoaac 0:5af71b978fb7 88 GAIN_P = (float)Registar[MotorP]/10;
kikoaac 0:5af71b978fb7 89 GAIN_I = (float)Registar[MotorI]/10;
kikoaac 0:5af71b978fb7 90 GAIN_D = (float)Registar[MotorD]/10;
kikoaac 0:5af71b978fb7 91 //printf("Rotate::: %d ,%d ,%d\n",pulse, Registar[MoterPulseH-1], Registar[MoterPulseH]);
kikoaac 0:5af71b978fb7 92 }
kikoaac 0:5af71b978fb7 93 /*if(flag==1)
kikoaac 0:5af71b978fb7 94 {
kikoaac 0:5af71b978fb7 95 static char Reg;
kikoaac 0:5af71b978fb7 96 static short pul;
kikoaac 0:5af71b978fb7 97 if(Registar[RotateMode]!=Reg||short(Registar[PulsePerRev]<<8|Registar[PulsePerRev2])!=pul)
kikoaac 0:5af71b978fb7 98 flag=0;
kikoaac 0:5af71b978fb7 99 }*/
kikoaac 0:5af71b978fb7 100 }
kikoaac 0:5af71b978fb7 101 void AngleStay(float point , float mypoint ,bool mode)
kikoaac 0:5af71b978fb7 102 {
kikoaac 0:5af71b978fb7 103 float sa;
kikoaac 0:5af71b978fb7 104 if(mode==1) sa = (int)point%360-(int)mypoint%360;
kikoaac 0:5af71b978fb7 105 else sa = point-mypoint;
kikoaac 0:5af71b978fb7 106 if(sa>10)
kikoaac 0:5af71b978fb7 107 motor.run(Front,1);
kikoaac 0:5af71b978fb7 108 else if(sa<-10)
kikoaac 0:5af71b978fb7 109 motor.run(Back,1);
kikoaac 0:5af71b978fb7 110 else motor.run(Stop,1);
kikoaac 0:5af71b978fb7 111
kikoaac 0:5af71b978fb7 112 printf("%f %f %f \n",mypoint,point,sa);
kikoaac 0:5af71b978fb7 113 }
kikoaac 0:5af71b978fb7 114 void AngleStay_PID(float point , float mypoint)
kikoaac 0:5af71b978fb7 115 {
kikoaac 0:5af71b978fb7 116 float x = PIctrl(point , mypoint);
kikoaac 0:5af71b978fb7 117 //pid.dPoint = mypoint;
kikoaac 0:5af71b978fb7 118 //pid.point = pid.PIDval;//wheel1.getSumangle();
kikoaac 0:5af71b978fb7 119 //float x = pid.PIDval;
kikoaac 0:5af71b978fb7 120 motor = x/5000;
kikoaac 0:5af71b978fb7 121 }
kikoaac 0:5af71b978fb7 122 void SpeedStay();
kikoaac 0:5af71b978fb7 123 void SpeedStay_PID();
kikoaac 0:5af71b978fb7 124
kikoaac 0:5af71b978fb7 125 void Motor_mode()
kikoaac 0:5af71b978fb7 126 {
kikoaac 0:5af71b978fb7 127 char IF = Registar[MotorMode]&(~0x80);
kikoaac 0:5af71b978fb7 128 switch(IF)
kikoaac 0:5af71b978fb7 129 {
kikoaac 0:5af71b978fb7 130 case 1:ManiacMotor_Mode(short(Registar[MotorPWM]<<8|Registar[MotorPWM2]),Registar[MotorState]);break;
kikoaac 0:5af71b978fb7 131 case 2:AngleStay(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),wheel.getSumangle(),Registar[MotorMode]>>7);break;
kikoaac 0:5af71b978fb7 132 case 3:AngleStay_PID(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),wheel.getSumangle());break;
kikoaac 0:5af71b978fb7 133 //case 4:SpeedStay();break;
kikoaac 0:5af71b978fb7 134 //case 5:SpeedStay_PID();break;
kikoaac 0:5af71b978fb7 135 default:motor.run(0,Stop);
kikoaac 0:5af71b978fb7 136 }
kikoaac 0:5af71b978fb7 137 //motor.run(Registar[MotorState],float(Registar[MotorPWM])/256.0);
kikoaac 0:5af71b978fb7 138 /*motor=float(Registar[MotorPWM]<<8|Registar[MotorPWM2]-32768)/32768.0;
kikoaac 0:5af71b978fb7 139 printf("%f\n",float(Registar[MotorPWM]<<8|Registar[MotorPWM2]-32768)/32768.0);*/
kikoaac 0:5af71b978fb7 140 }
kikoaac 0:5af71b978fb7 141
kikoaac 0:5af71b978fb7 142 void encode(/*void const *argument*/)
kikoaac 0:5af71b978fb7 143 {
kikoaac 0:5af71b978fb7 144 //wheel.encode();
kikoaac 0:5af71b978fb7 145 Motor_mode();
kikoaac 0:5af71b978fb7 146 Rotate();
kikoaac 0:5af71b978fb7 147 }
kikoaac 0:5af71b978fb7 148 //void timer(){
kikoaac 0:5af71b978fb7 149 extern "C" void execute_spi_slave_hw( void )
kikoaac 0:5af71b978fb7 150 {
kikoaac 0:5af71b978fb7 151 //ledDbg = 1;
kikoaac 0:5af71b978fb7 152 wheel.state(1);
kikoaac 0:5af71b978fb7 153 if(i2c->receive()==I2CSlave::WriteAddressed&&mode==I2C_MODE)
kikoaac 0:5af71b978fb7 154 {
kikoaac 0:5af71b978fb7 155 LED=Red;
kikoaac 0:5af71b978fb7 156 //encoder.stop();
kikoaac 0:5af71b978fb7 157 //wheel->state(1);
kikoaac 0:5af71b978fb7 158 char DATA[2] = {};
kikoaac 0:5af71b978fb7 159 i2c->read(DATA,2);
kikoaac 0:5af71b978fb7 160 char reg=DATA[0];
kikoaac 0:5af71b978fb7 161 char num =DATA[1];
kikoaac 0:5af71b978fb7 162 char X[num];
kikoaac 0:5af71b978fb7 163 char f=0;
kikoaac 0:5af71b978fb7 164 //wait_us(1000);
kikoaac 0:5af71b978fb7 165 printf("R registar %d ",reg);
kikoaac 0:5af71b978fb7 166 switch(i2c->receive())
kikoaac 0:5af71b978fb7 167 {
kikoaac 0:5af71b978fb7 168 case 0 :break;
kikoaac 0:5af71b978fb7 169 case I2CSlave::ReadAddressed:
kikoaac 0:5af71b978fb7 170 {
kikoaac 0:5af71b978fb7 171 char *po = Registar+reg;
kikoaac 0:5af71b978fb7 172 for(int i=0;i<num;i++)
kikoaac 0:5af71b978fb7 173 X[i]=*po+i;
kikoaac 0:5af71b978fb7 174 i2c->write(X,num);
kikoaac 0:5af71b978fb7 175 f=1;
kikoaac 0:5af71b978fb7 176 do
kikoaac 0:5af71b978fb7 177 {
kikoaac 0:5af71b978fb7 178 f = i2c->write(Registar[reg]);
kikoaac 0:5af71b978fb7 179 // printf(" %d ",Registar[reg]);
kikoaac 0:5af71b978fb7 180 reg++;
kikoaac 0:5af71b978fb7 181 }while(f==1);
kikoaac 0:5af71b978fb7 182 break;
kikoaac 0:5af71b978fb7 183 }
kikoaac 0:5af71b978fb7 184 case I2CSlave::WriteGeneral:{break;}
kikoaac 0:5af71b978fb7 185 case I2CSlave::WriteAddressed:
kikoaac 0:5af71b978fb7 186 {
kikoaac 0:5af71b978fb7 187 char num = DATA[1];
kikoaac 0:5af71b978fb7 188 for(int i=1; i<num; i++,reg++)
kikoaac 0:5af71b978fb7 189 char X[num];
kikoaac 0:5af71b978fb7 190 i2c->read(X,num);
kikoaac 0:5af71b978fb7 191 //Registar[reg]=D;
kikoaac 0:5af71b978fb7 192 for (int i=0;i<num;i++)
kikoaac 0:5af71b978fb7 193 {
kikoaac 0:5af71b978fb7 194 Registar[reg]=X[i];
kikoaac 0:5af71b978fb7 195 // printf("%d ",Registar[reg]);
kikoaac 0:5af71b978fb7 196 reg++;
kikoaac 0:5af71b978fb7 197 }
kikoaac 0:5af71b978fb7 198 //printf(" Registar : %d ,%d\n",Registar[reg],reg);
kikoaac 0:5af71b978fb7 199
kikoaac 0:5af71b978fb7 200 break;
kikoaac 0:5af71b978fb7 201 }
kikoaac 0:5af71b978fb7 202 }
kikoaac 0:5af71b978fb7 203 //printf("OK\n");
kikoaac 0:5af71b978fb7 204 wheel.state(0);
kikoaac 0:5af71b978fb7 205 }
kikoaac 0:5af71b978fb7 206 if(spi->receive()&&mode==SPI_MODE) {
kikoaac 0:5af71b978fb7 207 LED=Blue|Red;
kikoaac 0:5af71b978fb7 208 //wheel.state(1);
kikoaac 0:5af71b978fb7 209 //rotateT.detach();
kikoaac 0:5af71b978fb7 210 //encoder.stop();
kikoaac 0:5af71b978fb7 211 char flag=1;
kikoaac 0:5af71b978fb7 212 char reg = spi->read();
kikoaac 0:5af71b978fb7 213 wait_us(50);
kikoaac 0:5af71b978fb7 214 char num = spi->read();
kikoaac 0:5af71b978fb7 215 //printf("SIZE %d\n",num);
kikoaac 0:5af71b978fb7 216 if(reg&0x80){
kikoaac 0:5af71b978fb7 217 reg=reg&(~0x80);
kikoaac 0:5af71b978fb7 218 flag=0;
kikoaac 0:5af71b978fb7 219 spi->reply(Registar[reg]);
kikoaac 0:5af71b978fb7 220 }
kikoaac 0:5af71b978fb7 221 else spi->reply(0x00);
kikoaac 0:5af71b978fb7 222 //wait_us(10);
kikoaac 0:5af71b978fb7 223
kikoaac 0:5af71b978fb7 224 if(flag)
kikoaac 0:5af71b978fb7 225 for(int i=0;i<num;i++)
kikoaac 0:5af71b978fb7 226 {
kikoaac 0:5af71b978fb7 227 while(!spi->receive());
kikoaac 0:5af71b978fb7 228 wait_us(50);
kikoaac 0:5af71b978fb7 229 Registar[reg+i] = spi->read();
kikoaac 0:5af71b978fb7 230 //printf("%d,%d\n",reg+i,Registar[reg+i]);
kikoaac 0:5af71b978fb7 231 }
kikoaac 0:5af71b978fb7 232 else
kikoaac 0:5af71b978fb7 233 {
kikoaac 0:5af71b978fb7 234 for(int i=0;i<num;i++)
kikoaac 0:5af71b978fb7 235 {
kikoaac 0:5af71b978fb7 236 //
kikoaac 0:5af71b978fb7 237 while(!spi->receive());
kikoaac 0:5af71b978fb7 238 //wait_us(50);
kikoaac 0:5af71b978fb7 239 spi->reply(Registar[reg++]);
kikoaac 0:5af71b978fb7 240 char dummy = spi->read();
kikoaac 0:5af71b978fb7 241 //printf("%d,%d\n",reg+i,Registar[reg+i]);
kikoaac 0:5af71b978fb7 242 }
kikoaac 0:5af71b978fb7 243
kikoaac 0:5af71b978fb7 244 }
kikoaac 0:5af71b978fb7 245 //printf("%d , %d\n",reg,Registar[reg]);
kikoaac 0:5af71b978fb7 246 flag=1;
kikoaac 0:5af71b978fb7 247 //spi->reply(00);
kikoaac 0:5af71b978fb7 248
kikoaac 0:5af71b978fb7 249 //wheel.state(0);
kikoaac 0:5af71b978fb7 250 //encoder.start(10);
kikoaac 0:5af71b978fb7 251
kikoaac 0:5af71b978fb7 252 }
kikoaac 0:5af71b978fb7 253 wheel.state(0);
kikoaac 0:5af71b978fb7 254
kikoaac 0:5af71b978fb7 255
kikoaac 0:5af71b978fb7 256 }
kikoaac 0:5af71b978fb7 257 int main() {
kikoaac 0:5af71b978fb7 258 //pc.baud(230400);
kikoaac 0:5af71b978fb7 259 Mode = new DigitalIn(MODE);
kikoaac 0:5af71b978fb7 260 Registar[0x60]=10;
kikoaac 0:5af71b978fb7 261 Registar[0x61]=32;
kikoaac 0:5af71b978fb7 262 //rotateN.attach(&enc,0.001);
kikoaac 0:5af71b978fb7 263 //Motor=0x08|0x01;
kikoaac 0:5af71b978fb7 264 if(*Mode==1)
kikoaac 0:5af71b978fb7 265 {
kikoaac 0:5af71b978fb7 266 spi = new SPISlave(MOSI, MISO, SCK,SSEL);
kikoaac 0:5af71b978fb7 267 spi->format(8,1);
kikoaac 0:5af71b978fb7 268 spi->frequency(4000000);
kikoaac 0:5af71b978fb7 269 spi->reply(0x00); // Prime SPI with first reply
kikoaac 0:5af71b978fb7 270 mode=SPI_MODE;
kikoaac 0:5af71b978fb7 271 LED=Blue;
kikoaac 0:5af71b978fb7 272 }
kikoaac 0:5af71b978fb7 273 else if(*Mode==0)
kikoaac 0:5af71b978fb7 274 {
kikoaac 0:5af71b978fb7 275 i2c = new I2CSlave(SDA,SCL);
kikoaac 0:5af71b978fb7 276 i2c->frequency(200000);
kikoaac 0:5af71b978fb7 277 //char address[4]={0x20,0xb2,0xee,0xf4};
kikoaac 0:5af71b978fb7 278 Address = new BusIn(I2C_addr_L,I2C_addr_H);
kikoaac 0:5af71b978fb7 279 i2c->address(0xa0/*address[Address->read()]*/);
kikoaac 0:5af71b978fb7 280 mode=I2C_MODE;
kikoaac 0:5af71b978fb7 281 LED=Green;
kikoaac 0:5af71b978fb7 282 delete Address ;
kikoaac 0:5af71b978fb7 283 }
kikoaac 0:5af71b978fb7 284 NVIC_SetVector( I2C_IRQn , ( uint32_t ) execute_spi_slave_hw ) ;
kikoaac 0:5af71b978fb7 285 NVIC_SetPriority( I2C_IRQn , 20 ) ;
kikoaac 0:5af71b978fb7 286 NVIC_EnableIRQ( I2C_IRQn ) ;
kikoaac 0:5af71b978fb7 287
kikoaac 0:5af71b978fb7 288 NVIC_SetPriority(TIMER_16_0_IRQn,9);
kikoaac 0:5af71b978fb7 289 NVIC_SetPriority(TIMER_16_1_IRQn,8);
kikoaac 0:5af71b978fb7 290 NVIC_SetPriority(TIMER_32_0_IRQn,7);
kikoaac 0:5af71b978fb7 291 NVIC_SetPriority(TIMER_32_1_IRQn,6);
kikoaac 0:5af71b978fb7 292 rotateT.attach(&encode,0.01);
kikoaac 0:5af71b978fb7 293
kikoaac 0:5af71b978fb7 294 NVIC_SetPriority( EINT0_IRQn , 5 ) ;
kikoaac 0:5af71b978fb7 295 NVIC_SetPriority( EINT1_IRQn , 4 ) ;
kikoaac 0:5af71b978fb7 296 NVIC_SetPriority( EINT2_IRQn , 3 ) ;
kikoaac 0:5af71b978fb7 297 NVIC_SetPriority( EINT3_IRQn , 2 ) ;
kikoaac 0:5af71b978fb7 298 while(1) {
kikoaac 0:5af71b978fb7 299
kikoaac 0:5af71b978fb7 300 /*Motor_mode();
kikoaac 0:5af71b978fb7 301 Rotate();
kikoaac 0:5af71b978fb7 302 encode();*/
kikoaac 0:5af71b978fb7 303 }
kikoaac 0:5af71b978fb7 304 }
kikoaac 0:5af71b978fb7 305
kikoaac 0:5af71b978fb7 306
kikoaac 0:5af71b978fb7 307