Kiko Ishimoto / Mbed 2 deprecated Nucleo_spi

ＭＢＥＤテスト

Dependencies:   Motor PID mbed

main.cpp@2:1194c29429bf, 2015-07-21 (annotated)

Committer:
kikoaac
Date:
Tue Jul 21 08:13:33 2015 +0000
Revision:
2:1194c29429bf
Parent:
0:5af71b978fb7 
MotorDrive

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kikoaac 0:5af71b978fb7 1 // Reply to a SPI master as slave
kikoaac 2:1194c29429bf 2 float GAIN_P = 4.3f; // 比例ｹﾞｲﾝ
kikoaac 2:1194c29429bf 3 float GAIN_I = 1.3f; // 積分ｹﾞｲﾝ
kikoaac 2:1194c29429bf 4 float GAIN_D = 1.21f;
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 2:1194c29429bf 11 #include "Registar.h"
kikoaac 2:1194c29429bf 12 BusOut LED(Red_Pin,Green_Pin,Blue_Pin);
kikoaac 2:1194c29429bf 13 class LEDMode
kikoaac 2:1194c29429bf 14 {
kikoaac 2:1194c29429bf 15 Ticker Tic;
kikoaac 2:1194c29429bf 16 private:
kikoaac 2:1194c29429bf 17 char led,Off;
kikoaac 2:1194c29429bf 18 void Inter()
kikoaac 2:1194c29429bf 19 {
kikoaac 2:1194c29429bf 20 static int flag;
kikoaac 2:1194c29429bf 21 flag++;
kikoaac 2:1194c29429bf 22 if(flag&2)
kikoaac 2:1194c29429bf 23 {
kikoaac 2:1194c29429bf 24 LED = led;
kikoaac 2:1194c29429bf 25 }
kikoaac 2:1194c29429bf 26 else
kikoaac 2:1194c29429bf 27 {
kikoaac 2:1194c29429bf 28 Tic.attach(this,&LEDMode::Inter,TransWait*0.3);
kikoaac 2:1194c29429bf 29 LED = Off;
kikoaac 2:1194c29429bf 30 }
kikoaac 2:1194c29429bf 31 }
kikoaac 2:1194c29429bf 32 public:
kikoaac 2:1194c29429bf 33 char Red,Blue,Green;
kikoaac 2:1194c29429bf 34 char TransWait;
kikoaac 2:1194c29429bf 35 char InterVal;
kikoaac 2:1194c29429bf 36 char Mode[7];// = {RED|BLUE,RED|GREEN,RED|GREEN,BLUE|GREEN,BLUE|GREEN};
kikoaac 2:1194c29429bf 37 float Int[7];
kikoaac 2:1194c29429bf 38 char OFF[7];
kikoaac 2:1194c29429bf 39 LEDMode()
kikoaac 2:1194c29429bf 40 {
kikoaac 2:1194c29429bf 41 char mode[7] = {RED,RED|BLUE,RED|GREEN,RED|GREEN,BLUE|GREEN,BLUE|GREEN,RED|BLUE};
kikoaac 2:1194c29429bf 42 char off[7] = {0,0,0,GREEN,0,GREEN,RED};
kikoaac 2:1194c29429bf 43 float in[7] = {1,1,1.5,1.7,1.5,1.7,1.0};
kikoaac 2:1194c29429bf 44 for(int i=0;i<7;i++)
kikoaac 2:1194c29429bf 45 {
kikoaac 2:1194c29429bf 46 Mode[i]=mode[i];
kikoaac 2:1194c29429bf 47 Int[i]=in[i];
kikoaac 2:1194c29429bf 48 OFF[i]=off[i];
kikoaac 2:1194c29429bf 49 }
kikoaac 2:1194c29429bf 50 Red=RED;
kikoaac 2:1194c29429bf 51 Blue=BLUE;
kikoaac 2:1194c29429bf 52 Green=GREEN;
kikoaac 2:1194c29429bf 53 Off=0;
kikoaac 2:1194c29429bf 54 }
kikoaac 2:1194c29429bf 55 void Wait(int i)
kikoaac 2:1194c29429bf 56 {
kikoaac 2:1194c29429bf 57 float in = 1.0;
kikoaac 2:1194c29429bf 58 TransWait = in;
kikoaac 2:1194c29429bf 59 if(i==0)led = BLUE;
kikoaac 2:1194c29429bf 60 else led = GREEN;
kikoaac 2:1194c29429bf 61 Off=0;
kikoaac 2:1194c29429bf 62 Tic.detach();
kikoaac 2:1194c29429bf 63 Tic.attach(this,&LEDMode::Inter,TransWait);
kikoaac 2:1194c29429bf 64 }
kikoaac 2:1194c29429bf 65 void Act(int i)
kikoaac 2:1194c29429bf 66 {
kikoaac 2:1194c29429bf 67 if(i>=6)i=6;
kikoaac 2:1194c29429bf 68 {
kikoaac 2:1194c29429bf 69 Off=OFF[i];
kikoaac 2:1194c29429bf 70 led = Mode[i];
kikoaac 2:1194c29429bf 71 TransWait = Int[i];
kikoaac 2:1194c29429bf 72 Tic.detach();
kikoaac 2:1194c29429bf 73 Tic.attach(this,&LEDMode::Inter,TransWait);
kikoaac 2:1194c29429bf 74 }
kikoaac 2:1194c29429bf 75 }
kikoaac 2:1194c29429bf 76 };
kikoaac 2:1194c29429bf 77 LEDMode Led;
kikoaac 2:1194c29429bf 78 PID pid(GAIN_P,GAIN_I,GAIN_D);
kikoaac 2:1194c29429bf 79 short PR;
kikoaac 2:1194c29429bf 80 short pulse ;
kikoaac 2:1194c29429bf 81 short Rev;
kikoaac 2:1194c29429bf 82 char Registar[0x80]={0};
kikoaac 0:5af71b978fb7 83 char mode;
kikoaac 2:1194c29429bf 84 /*double PIctrl(double dCommand, double dVal)
kikoaac 0:5af71b978fb7 85 {
kikoaac 0:5af71b978fb7 86 static double s_dErrIntg = 0 ,dErr_prev=0;
kikoaac 0:5af71b978fb7 87 double dErr;
kikoaac 0:5af71b978fb7 88 double dRet;
kikoaac 0:5af71b978fb7 89
kikoaac 0:5af71b978fb7 90 // 誤差
kikoaac 0:5af71b978fb7 91 dErr = dCommand - dVal;
kikoaac 0:5af71b978fb7 92
kikoaac 0:5af71b978fb7 93 // 誤差積分
kikoaac 0:5af71b978fb7 94 s_dErrIntg += (dErr+dErr_prev )* timer.read() /2.0;
kikoaac 0:5af71b978fb7 95
kikoaac 0:5af71b978fb7 96 // 制御入力
kikoaac 0:5af71b978fb7 97 dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*(dErr-dErr_prev)/timer.read();
kikoaac 2:1194c29429bf 98 //printf("%f ",timer.read());
kikoaac 0:5af71b978fb7 99 timer.reset();
kikoaac 2:1194c29429bf 100 //printf("%f ",dRet);
kikoaac 0:5af71b978fb7 101 dErr_prev = dErr;
kikoaac 0:5af71b978fb7 102 return (dRet);
kikoaac 2:1194c29429bf 103 }*/
kikoaac 0:5af71b978fb7 104 Ticker rotateT;
kikoaac 2:1194c29429bf 105 //Ticker rotateN;
kikoaac 2:1194c29429bf 106 //QEI wheel(Rotal_A,Rotal_B,NC,100,QEI::X4_ENCODING);
kikoaac 0:5af71b978fb7 107
kikoaac 0:5af71b978fb7 108
kikoaac 2:1194c29429bf 109 Motor motor(Motor_H1,Motor_L2,Motor_L1,Motor_H2,Motor_PWM,short(0xffff));
kikoaac 0:5af71b978fb7 110
kikoaac 2:1194c29429bf 111 void ManiacMotor_Mode(float duty,char mode)
kikoaac 0:5af71b978fb7 112 {
kikoaac 0:5af71b978fb7 113 motor.run(mode,duty);
kikoaac 0:5af71b978fb7 114 //motor=1;
kikoaac 2:1194c29429bf 115 //printf("%f %f\n " , duty,1.0/duty);
kikoaac 0:5af71b978fb7 116 }
kikoaac 0:5af71b978fb7 117 //PinName rotatepin[3];
kikoaac 0:5af71b978fb7 118 int rotatemode;
kikoaac 0:5af71b978fb7 119 void RotateSet()
kikoaac 0:5af71b978fb7 120 {
kikoaac 0:5af71b978fb7 121 //delete wheel;
kikoaac 2:1194c29429bf 122 //pulse = short(Registar[PulsePerRev]<<8|Registar[PulsePerRev2]);
kikoaac 0:5af71b978fb7 123 /*if(Registar[RotateMode]&0x01)rotatepin[0]=Rotal_A;
kikoaac 0:5af71b978fb7 124 //else rotatepin[0]=dp13;
kikoaac 0:5af71b978fb7 125 if(Registar[RotateMode]&0x02)rotatepin[1]=Rotal_B;
kikoaac 0:5af71b978fb7 126 //else rotatepin[0]=dp13;
kikoaac 0:5af71b978fb7 127 if(Registar[RotateMode]&0x04)rotatepin[2]=Rotal_Z;
kikoaac 0:5af71b978fb7 128 //else rotatepin[0]=dp13;*/
kikoaac 0:5af71b978fb7 129 if(Registar[RotateMode]&0x08)rotatemode=1;
kikoaac 0:5af71b978fb7 130 else if(!Registar[RotateMode]&0x08)rotatemode=0;
kikoaac 0:5af71b978fb7 131 if(Registar[RotateMode]&0x80)
kikoaac 0:5af71b978fb7 132 {
kikoaac 0:5af71b978fb7 133 //wheel.SetUp(rotatepin[0],rotatepin[1],rotatepin[2],pulse,rotatemode);
kikoaac 0:5af71b978fb7 134 //printf("make QEI\n");
kikoaac 0:5af71b978fb7 135 }
kikoaac 0:5af71b978fb7 136 }
kikoaac 0:5af71b978fb7 137 void Rotate(/*void const *argument*/)
kikoaac 0:5af71b978fb7 138 {
kikoaac 0:5af71b978fb7 139 static bool flag=0;
kikoaac 2:1194c29429bf 140
kikoaac 2:1194c29429bf 141 if(/*Registar[RotateMode]&0x80*/1)
kikoaac 0:5af71b978fb7 142 {
kikoaac 0:5af71b978fb7 143 if(!flag)
kikoaac 0:5af71b978fb7 144 {
kikoaac 0:5af71b978fb7 145 RotateSet();
kikoaac 0:5af71b978fb7 146 flag=1;
kikoaac 0:5af71b978fb7 147 }
kikoaac 2:1194c29429bf 148 pulse = short((Registar[PulsePerRev]<<8)|Registar[PulsePerRev+1]/*wheel.getPulses()*/);
kikoaac 2:1194c29429bf 149 Rev = short((Registar[PulsePerRev]<<8)|Registar[PulsePerRev]/*wheel.getRevolutions()*/);
kikoaac 2:1194c29429bf 150 PR = short((Registar[PulsePerSec]<<8)|Registar[PulsePerSec+1]/*wheel.getRPMS()*1000*/);
kikoaac 0:5af71b978fb7 151 Registar[MoterRevolutionH] = (Rev&0xff00)>>8;
kikoaac 0:5af71b978fb7 152 Registar[MoterRevolutionH-1] = (Rev&0xff);
kikoaac 0:5af71b978fb7 153 Registar[MoterPulseH] = (pulse&0xff00)>>8;
kikoaac 0:5af71b978fb7 154 Registar[MoterPulseH-1] = (pulse&0xff);
kikoaac 0:5af71b978fb7 155 Registar[MoterSpeedH] = (PR&0xff00)>>8;
kikoaac 0:5af71b978fb7 156 Registar[MoterSpeedH-1] = (PR&0xff);
kikoaac 2:1194c29429bf 157 char IF = Registar[MotorMode]&(~0x80);
kikoaac 2:1194c29429bf 158 if(IF == 3 || IF == 5)
kikoaac 2:1194c29429bf 159 {
kikoaac 2:1194c29429bf 160 pid.GAIN_P = (float)Registar[MotorP]/10;
kikoaac 2:1194c29429bf 161 pid.GAIN_I = (float)Registar[MotorI]/10;
kikoaac 2:1194c29429bf 162 pid.GAIN_D = (float)Registar[MotorD]/10;
kikoaac 2:1194c29429bf 163 //printf("PID P%f I%f D%f \n",pid.GAIN_P,pid.GAIN_I,pid.GAIN_D);
kikoaac 2:1194c29429bf 164 }
kikoaac 2:1194c29429bf 165 //printf("Rotate::: %d ,%d ,%d\n",pulse, Registar[PulsePerRev], Registar[PulsePerRev+1]);
kikoaac 0:5af71b978fb7 166 }
kikoaac 0:5af71b978fb7 167 /*if(flag==1)
kikoaac 0:5af71b978fb7 168 {
kikoaac 0:5af71b978fb7 169 static char Reg;
kikoaac 0:5af71b978fb7 170 static short pul;
kikoaac 0:5af71b978fb7 171 if(Registar[RotateMode]!=Reg||short(Registar[PulsePerRev]<<8|Registar[PulsePerRev2])!=pul)
kikoaac 0:5af71b978fb7 172 flag=0;
kikoaac 0:5af71b978fb7 173 }*/
kikoaac 0:5af71b978fb7 174 }
kikoaac 0:5af71b978fb7 175 void AngleStay(float point , float mypoint ,bool mode)
kikoaac 0:5af71b978fb7 176 {
kikoaac 0:5af71b978fb7 177 float sa;
kikoaac 0:5af71b978fb7 178 if(mode==1) sa = (int)point%360-(int)mypoint%360;
kikoaac 0:5af71b978fb7 179 else sa = point-mypoint;
kikoaac 0:5af71b978fb7 180 if(sa>10)
kikoaac 0:5af71b978fb7 181 motor.run(Front,1);
kikoaac 0:5af71b978fb7 182 else if(sa<-10)
kikoaac 0:5af71b978fb7 183 motor.run(Back,1);
kikoaac 0:5af71b978fb7 184 else motor.run(Stop,1);
kikoaac 0:5af71b978fb7 185
kikoaac 2:1194c29429bf 186 //printf("%f %f %f \n",mypoint,point,sa);
kikoaac 0:5af71b978fb7 187 }
kikoaac 2:1194c29429bf 188 void AngleStay_PID(float point , float mypoint,bool mode)
kikoaac 0:5af71b978fb7 189 {
kikoaac 2:1194c29429bf 190 //float x = PIctrl(point , mypoint);
kikoaac 2:1194c29429bf 191 float sa=0;
kikoaac 2:1194c29429bf 192 if(mode==1) sa = (int)point%360-(int)mypoint%360;
kikoaac 2:1194c29429bf 193 else sa = point-mypoint;
kikoaac 0:5af71b978fb7 194 //pid.dPoint = mypoint;
kikoaac 0:5af71b978fb7 195 //pid.point = pid.PIDval;//wheel1.getSumangle();
kikoaac 2:1194c29429bf 196 pid.dPoint = mypoint;
kikoaac 2:1194c29429bf 197 pid.dTarget = point;//wheel1.getSumangle();
kikoaac 2:1194c29429bf 198 float x = (float)pid.data;
kikoaac 0:5af71b978fb7 199 //float x = pid.PIDval;
kikoaac 0:5af71b978fb7 200 motor = x/5000;
kikoaac 2:1194c29429bf 201 //printf("%f %f %f \n",mypoint,point,x/5000);
kikoaac 0:5af71b978fb7 202 }
kikoaac 0:5af71b978fb7 203
kikoaac 2:1194c29429bf 204 void SpeedStay(float point , float mypoint ,int mode)
kikoaac 2:1194c29429bf 205 {
kikoaac 2:1194c29429bf 206 float sa;
kikoaac 2:1194c29429bf 207 static float duty=0.1;
kikoaac 2:1194c29429bf 208 int X[2] = {Front , Back};
kikoaac 2:1194c29429bf 209 sa = (point-mypoint);
kikoaac 2:1194c29429bf 210 float a = sa;
kikoaac 2:1194c29429bf 211 if(sa<0)sa*=-1;
kikoaac 2:1194c29429bf 212 if(a>0.3)
kikoaac 2:1194c29429bf 213 {
kikoaac 2:1194c29429bf 214 motor.run(X[mode],sa*duty/1000);
kikoaac 2:1194c29429bf 215 duty+=1;
kikoaac 2:1194c29429bf 216 }
kikoaac 2:1194c29429bf 217 else if(a<-0.3)
kikoaac 2:1194c29429bf 218 {
kikoaac 2:1194c29429bf 219 motor.run(X[mode],sa*duty/1000);
kikoaac 2:1194c29429bf 220 duty-=1;
kikoaac 2:1194c29429bf 221 }
kikoaac 2:1194c29429bf 222 else motor.run(X[mode],sa*duty/1000);
kikoaac 2:1194c29429bf 223
kikoaac 2:1194c29429bf 224 //printf("%f %f %f \n",mypoint,point,sa*duty/1000);
kikoaac 2:1194c29429bf 225 }
kikoaac 2:1194c29429bf 226 void SpeedStay_PID(float point , float mypoint,int mode)
kikoaac 2:1194c29429bf 227 {
kikoaac 2:1194c29429bf 228 float x = 0;
kikoaac 2:1194c29429bf 229 //x = PIctrl(point , mypoint);
kikoaac 2:1194c29429bf 230 if(x<0)x*=-1;
kikoaac 2:1194c29429bf 231 //if(point<0)point*=-1;
kikoaac 2:1194c29429bf 232 int X[2] = {Front , Back};
kikoaac 2:1194c29429bf 233 pid.dPoint = mypoint;
kikoaac 2:1194c29429bf 234 pid.dTarget = point;//wheel1.getSumangle();
kikoaac 2:1194c29429bf 235 x = (float)pid.data/1000;
kikoaac 2:1194c29429bf 236 if(x<0.001&&x>-0.001)motor.run(Stop,x);
kikoaac 2:1194c29429bf 237 else motor.run(X[mode],x);
kikoaac 2:1194c29429bf 238 //printf("%f %f %f %i\n",mypoint,point,x/1000,mode);
kikoaac 2:1194c29429bf 239 }
kikoaac 2:1194c29429bf 240 char Inflag=0;
kikoaac 0:5af71b978fb7 241 void Motor_mode()
kikoaac 0:5af71b978fb7 242 {
kikoaac 2:1194c29429bf 243 static char a=0;
kikoaac 0:5af71b978fb7 244 char IF = Registar[MotorMode]&(~0x80);
kikoaac 0:5af71b978fb7 245 switch(IF)
kikoaac 0:5af71b978fb7 246 {
kikoaac 2:1194c29429bf 247 case 0:motor.run(Free,0);break;
kikoaac 2:1194c29429bf 248 case 1:ManiacMotor_Mode(float((int(Registar[MotorPWM])<<8)|Registar[MotorPWM2])/0xffff,Registar[MotorState]);break;
kikoaac 2:1194c29429bf 249 case 2:AngleStay(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),pulse*Registar[PulsePerAngle],Registar[MotorMode]>>7);break;
kikoaac 2:1194c29429bf 250 case 3:AngleStay_PID(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),pulse*Registar[PulsePerAngle],Registar[MotorMode]>>7);break;
kikoaac 2:1194c29429bf 251 case 4:SpeedStay(short(Registar[TargetSpeed]<<8|Registar[TargetSpeed2]),(float)PR*Registar[PulsePerAngle]/1000,int((Registar[TargetSpeed]>>7)==1));break;
kikoaac 2:1194c29429bf 252 case 5:SpeedStay_PID(short(Registar[TargetSpeed]<<8|Registar[TargetSpeed2]),(float)PR*Registar[PulsePerAngle]/1000,int((Registar[TargetSpeed]>>7)==1));break;
kikoaac 2:1194c29429bf 253 default:motor.run(Stop,0xffff);
kikoaac 0:5af71b978fb7 254 }
kikoaac 2:1194c29429bf 255 static bool bo=false;
kikoaac 2:1194c29429bf 256 if(!(IF==3||IF==5)&&bo==false)
kikoaac 2:1194c29429bf 257 {
kikoaac 2:1194c29429bf 258 //t.stop();
kikoaac 2:1194c29429bf 259 //printf("STOP\n");
kikoaac 2:1194c29429bf 260 pid.stop();
kikoaac 2:1194c29429bf 261 pid.s_dErrIntg = 0;
kikoaac 2:1194c29429bf 262 bo=true;
kikoaac 2:1194c29429bf 263 }
kikoaac 2:1194c29429bf 264 else if((IF==3||IF==5)&&bo==true)
kikoaac 2:1194c29429bf 265 {
kikoaac 2:1194c29429bf 266 bo=false;
kikoaac 2:1194c29429bf 267 //printf("START\n");
kikoaac 2:1194c29429bf 268 //t.stop();
kikoaac 2:1194c29429bf 269 pid.Start();
kikoaac 2:1194c29429bf 270 }
kikoaac 2:1194c29429bf 271 //printf("%d",IF);
kikoaac 2:1194c29429bf 272 if(IF!=a)
kikoaac 2:1194c29429bf 273 Led.Act((int)IF);
kikoaac 2:1194c29429bf 274 a=IF;
kikoaac 0:5af71b978fb7 275 //motor.run(Registar[MotorState],float(Registar[MotorPWM])/256.0);
kikoaac 0:5af71b978fb7 276 /*motor=float(Registar[MotorPWM]<<8|Registar[MotorPWM2]-32768)/32768.0;
kikoaac 0:5af71b978fb7 277 printf("%f\n",float(Registar[MotorPWM]<<8|Registar[MotorPWM2]-32768)/32768.0);*/
kikoaac 0:5af71b978fb7 278 }
kikoaac 0:5af71b978fb7 279
kikoaac 2:1194c29429bf 280 void Tic(/*void const *argument*/)
kikoaac 0:5af71b978fb7 281 {
kikoaac 0:5af71b978fb7 282 Motor_mode();
kikoaac 0:5af71b978fb7 283 Rotate();
kikoaac 2:1194c29429bf 284 //printf("Tic\n");
kikoaac 0:5af71b978fb7 285 }
kikoaac 0:5af71b978fb7 286 //void timer(){
kikoaac 0:5af71b978fb7 287 extern "C" void execute_spi_slave_hw( void )
kikoaac 0:5af71b978fb7 288 {
kikoaac 0:5af71b978fb7 289 //ledDbg = 1;
kikoaac 2:1194c29429bf 290 //wheel.state(1);
kikoaac 0:5af71b978fb7 291 if(i2c->receive()==I2CSlave::WriteAddressed&&mode==I2C_MODE)
kikoaac 2:1194c29429bf 292 {
kikoaac 2:1194c29429bf 293 //LED=Red;
kikoaac 0:5af71b978fb7 294 //encoder.stop();
kikoaac 0:5af71b978fb7 295 //wheel->state(1);
kikoaac 0:5af71b978fb7 296 char DATA[2] = {};
kikoaac 0:5af71b978fb7 297 i2c->read(DATA,2);
kikoaac 0:5af71b978fb7 298 char reg=DATA[0];
kikoaac 0:5af71b978fb7 299 char num =DATA[1];
kikoaac 0:5af71b978fb7 300 char X[num];
kikoaac 0:5af71b978fb7 301 char f=0;
kikoaac 2:1194c29429bf 302 wait_us(1000);
kikoaac 2:1194c29429bf 303 //printf("R registar %d \n",reg);
kikoaac 0:5af71b978fb7 304 switch(i2c->receive())
kikoaac 0:5af71b978fb7 305 {
kikoaac 0:5af71b978fb7 306 case 0 :break;
kikoaac 0:5af71b978fb7 307 case I2CSlave::ReadAddressed:
kikoaac 0:5af71b978fb7 308 {
kikoaac 2:1194c29429bf 309 //char *po = Registar+reg;
kikoaac 0:5af71b978fb7 310 for(int i=0;i<num;i++)
kikoaac 2:1194c29429bf 311 {
kikoaac 2:1194c29429bf 312 //char a=*po+i;
kikoaac 2:1194c29429bf 313 X[i] = Registar[reg+i];
kikoaac 2:1194c29429bf 314 }
kikoaac 0:5af71b978fb7 315 i2c->write(X,num);
kikoaac 2:1194c29429bf 316 /*f=1;
kikoaac 0:5af71b978fb7 317 do
kikoaac 0:5af71b978fb7 318 {
kikoaac 0:5af71b978fb7 319 f = i2c->write(Registar[reg]);
kikoaac 0:5af71b978fb7 320 // printf(" %d ",Registar[reg]);
kikoaac 0:5af71b978fb7 321 reg++;
kikoaac 2:1194c29429bf 322 }while(f==1);*/
kikoaac 0:5af71b978fb7 323 break;
kikoaac 0:5af71b978fb7 324 }
kikoaac 2:1194c29429bf 325 case I2CSlave::WriteGeneral:{
kikoaac 2:1194c29429bf 326
kikoaac 2:1194c29429bf 327 break;
kikoaac 2:1194c29429bf 328 }
kikoaac 0:5af71b978fb7 329 case I2CSlave::WriteAddressed:
kikoaac 0:5af71b978fb7 330 {
kikoaac 0:5af71b978fb7 331 char num = DATA[1];
kikoaac 0:5af71b978fb7 332 i2c->read(X,num);
kikoaac 0:5af71b978fb7 333 //Registar[reg]=D;
kikoaac 0:5af71b978fb7 334 for (int i=0;i<num;i++)
kikoaac 0:5af71b978fb7 335 {
kikoaac 0:5af71b978fb7 336 Registar[reg]=X[i];
kikoaac 0:5af71b978fb7 337 // printf("%d ",Registar[reg]);
kikoaac 2:1194c29429bf 338 //printf(" Registar : %d ,%d\n",Registar[reg],reg);
kikoaac 0:5af71b978fb7 339 reg++;
kikoaac 0:5af71b978fb7 340 }
kikoaac 0:5af71b978fb7 341 //printf(" Registar : %d ,%d\n",Registar[reg],reg);
kikoaac 0:5af71b978fb7 342 break;
kikoaac 0:5af71b978fb7 343 }
kikoaac 0:5af71b978fb7 344 }
kikoaac 0:5af71b978fb7 345 //printf("OK\n");
kikoaac 2:1194c29429bf 346 //wheel.state(0);
kikoaac 0:5af71b978fb7 347 }
kikoaac 0:5af71b978fb7 348 if(spi->receive()&&mode==SPI_MODE) {
kikoaac 2:1194c29429bf 349 //LED=Blue|Red;
kikoaac 0:5af71b978fb7 350 //wheel.state(1);
kikoaac 0:5af71b978fb7 351 //rotateT.detach();
kikoaac 0:5af71b978fb7 352 //encoder.stop();
kikoaac 0:5af71b978fb7 353 char flag=1;
kikoaac 0:5af71b978fb7 354 char reg = spi->read();
kikoaac 0:5af71b978fb7 355 wait_us(50);
kikoaac 0:5af71b978fb7 356 char num = spi->read();
kikoaac 0:5af71b978fb7 357 //printf("SIZE %d\n",num);
kikoaac 0:5af71b978fb7 358 if(reg&0x80){
kikoaac 0:5af71b978fb7 359 reg=reg&(~0x80);
kikoaac 0:5af71b978fb7 360 flag=0;
kikoaac 0:5af71b978fb7 361 spi->reply(Registar[reg]);
kikoaac 0:5af71b978fb7 362 }
kikoaac 0:5af71b978fb7 363 else spi->reply(0x00);
kikoaac 0:5af71b978fb7 364 //wait_us(10);
kikoaac 0:5af71b978fb7 365
kikoaac 0:5af71b978fb7 366 if(flag)
kikoaac 0:5af71b978fb7 367 for(int i=0;i<num;i++)
kikoaac 0:5af71b978fb7 368 {
kikoaac 0:5af71b978fb7 369 while(!spi->receive());
kikoaac 0:5af71b978fb7 370 wait_us(50);
kikoaac 0:5af71b978fb7 371 Registar[reg+i] = spi->read();
kikoaac 0:5af71b978fb7 372 //printf("%d,%d\n",reg+i,Registar[reg+i]);
kikoaac 0:5af71b978fb7 373 }
kikoaac 0:5af71b978fb7 374 else
kikoaac 0:5af71b978fb7 375 {
kikoaac 0:5af71b978fb7 376 for(int i=0;i<num;i++)
kikoaac 0:5af71b978fb7 377 {
kikoaac 0:5af71b978fb7 378 //
kikoaac 0:5af71b978fb7 379 while(!spi->receive());
kikoaac 0:5af71b978fb7 380 //wait_us(50);
kikoaac 0:5af71b978fb7 381 spi->reply(Registar[reg++]);
kikoaac 0:5af71b978fb7 382 char dummy = spi->read();
kikoaac 0:5af71b978fb7 383 //printf("%d,%d\n",reg+i,Registar[reg+i]);
kikoaac 0:5af71b978fb7 384 }
kikoaac 0:5af71b978fb7 385
kikoaac 0:5af71b978fb7 386 }
kikoaac 0:5af71b978fb7 387 //printf("%d , %d\n",reg,Registar[reg]);
kikoaac 0:5af71b978fb7 388 flag=1;
kikoaac 0:5af71b978fb7 389 //spi->reply(00);
kikoaac 0:5af71b978fb7 390
kikoaac 0:5af71b978fb7 391 //wheel.state(0);
kikoaac 0:5af71b978fb7 392 //encoder.start(10);
kikoaac 2:1194c29429bf 393 wait(.01);
kikoaac 0:5af71b978fb7 394 }
kikoaac 2:1194c29429bf 395 /*NVIC_ClearPendingIRQ (TIMER_16_0_IRQn);
kikoaac 2:1194c29429bf 396 NVIC_ClearPendingIRQ (TIMER_16_1_IRQn);
kikoaac 2:1194c29429bf 397 NVIC_ClearPendingIRQ (TIMER_32_0_IRQn);
kikoaac 2:1194c29429bf 398 NVIC_ClearPendingIRQ (TIMER_32_1_IRQn);*/
kikoaac 2:1194c29429bf 399 //wheel.state(0);
kikoaac 2:1194c29429bf 400 //encode();
kikoaac 2:1194c29429bf 401 Inflag=1;
kikoaac 2:1194c29429bf 402 }
kikoaac 2:1194c29429bf 403 extern "C" void HardFault_Handler() {
kikoaac 2:1194c29429bf 404 printf("Hard Fault!\n");
kikoaac 2:1194c29429bf 405 while(1);
kikoaac 0:5af71b978fb7 406 }
kikoaac 0:5af71b978fb7 407 int main() {
kikoaac 0:5af71b978fb7 408 //pc.baud(230400);
kikoaac 0:5af71b978fb7 409 Mode = new DigitalIn(MODE);
kikoaac 2:1194c29429bf 410 Registar[Who_am_I] = 0x67;
kikoaac 2:1194c29429bf 411 Registar[MotorP] = GAIN_P;
kikoaac 2:1194c29429bf 412 Registar[MotorI] = GAIN_I;
kikoaac 2:1194c29429bf 413 Registar[MotorD] = GAIN_D;
kikoaac 0:5af71b978fb7 414 //rotateN.attach(&enc,0.001);
kikoaac 0:5af71b978fb7 415 //Motor=0x08|0x01;
kikoaac 0:5af71b978fb7 416 if(*Mode==1)
kikoaac 0:5af71b978fb7 417 {
kikoaac 0:5af71b978fb7 418 spi = new SPISlave(MOSI, MISO, SCK,SSEL);
kikoaac 0:5af71b978fb7 419 spi->format(8,1);
kikoaac 0:5af71b978fb7 420 spi->frequency(4000000);
kikoaac 0:5af71b978fb7 421 spi->reply(0x00); // Prime SPI with first reply
kikoaac 0:5af71b978fb7 422 mode=SPI_MODE;
kikoaac 2:1194c29429bf 423 Led.Wait(0);
kikoaac 0:5af71b978fb7 424 }
kikoaac 0:5af71b978fb7 425 else if(*Mode==0)
kikoaac 0:5af71b978fb7 426 {
kikoaac 0:5af71b978fb7 427 i2c = new I2CSlave(SDA,SCL);
kikoaac 2:1194c29429bf 428 i2c->frequency(2000000);
kikoaac 2:1194c29429bf 429 char address[4]={0x02,0x04,0x06,0x08};
kikoaac 0:5af71b978fb7 430 Address = new BusIn(I2C_addr_L,I2C_addr_H);
kikoaac 0:5af71b978fb7 431 i2c->address(0xa0/*address[Address->read()]*/);
kikoaac 0:5af71b978fb7 432 mode=I2C_MODE;
kikoaac 2:1194c29429bf 433 Led.Wait(1);
kikoaac 0:5af71b978fb7 434 delete Address ;
kikoaac 0:5af71b978fb7 435 }
kikoaac 0:5af71b978fb7 436 NVIC_SetVector( I2C_IRQn , ( uint32_t ) execute_spi_slave_hw ) ;
kikoaac 2:1194c29429bf 437 NVIC_SetPriority( I2C_IRQn , 1) ;
kikoaac 0:5af71b978fb7 438 NVIC_EnableIRQ( I2C_IRQn ) ;
kikoaac 0:5af71b978fb7 439
kikoaac 2:1194c29429bf 440 NVIC_SetPriority(TIMER_16_0_IRQn,3);
kikoaac 2:1194c29429bf 441 NVIC_SetPriority(TIMER_16_1_IRQn,4);
kikoaac 2:1194c29429bf 442 NVIC_SetPriority(TIMER_32_0_IRQn,5);
kikoaac 0:5af71b978fb7 443 NVIC_SetPriority(TIMER_32_1_IRQn,6);
kikoaac 2:1194c29429bf 444 rotateT.attach(&Tic,0.005);
kikoaac 0:5af71b978fb7 445 while(1) {
kikoaac 2:1194c29429bf 446
kikoaac 2:1194c29429bf 447 //motor.run(1,1);
kikoaac 2:1194c29429bf 448 // Motor_mode();
kikoaac 2:1194c29429bf 449 // Rotate();
kikoaac 2:1194c29429bf 450 /*while(10-ti.read_ms()>0)
kikoaac 2:1194c29429bf 451 {}
kikoaac 2:1194c29429bf 452 ti.reset();
kikoaac 2:1194c29429bf 453 encode();*/
kikoaac 2:1194c29429bf 454
kikoaac 0:5af71b978fb7 455 }
kikoaac 0:5af71b978fb7 456 }
kikoaac 0:5af71b978fb7 457
kikoaac 0:5af71b978fb7 458
kikoaac 0:5af71b978fb7 459
kikoaac 2:1194c29429bf 460