FRA221_2015 / Mbed 2 deprecated 14B_DogPID_2

section B 57340500043 57340500046

Dependencies:   Motor PID eeprom mbed

Fork of Tanginamo by Digital B14

main.cpp@5:4168693bfb80, 2015-12-09 (annotated)

Committer:
ParinyaT
Date:
Wed Dec 09 00:01:34 2015 +0000
Revision:
5:4168693bfb80
Parent:
4:6e29193d7f95 
Fra 221 B14 (43,46)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ParinyaT 0:451c27e4d55e 1 //*****************************************************/
Tiz 1:9f279c68ed0c 2 // Include //
ParinyaT 0:451c27e4d55e 3 #include "mbed.h"
ParinyaT 0:451c27e4d55e 4 #include "pinconfig.h"
ParinyaT 0:451c27e4d55e 5 #include "PID.h"
ParinyaT 0:451c27e4d55e 6 #include "Motor.h"
ParinyaT 0:451c27e4d55e 7 #include "eeprom.h"
ParinyaT 0:451c27e4d55e 8
ParinyaT 0:451c27e4d55e 9 //*****************************************************/
ParinyaT 5:4168693bfb80 10 // PID parameter //
ParinyaT 5:4168693bfb80 11 float Rate = 0.001;
ParinyaT 5:4168693bfb80 12 float Kc = 0.4;
ParinyaT 5:4168693bfb80 13 float Ti = 0.15;
ParinyaT 5:4168693bfb80 14 float Td = 0.0;
ParinyaT 5:4168693bfb80 15 float Ki;
ParinyaT 5:4168693bfb80 16 float Kd;
ParinyaT 0:451c27e4d55e 17 //*****************************************************/
ParinyaT 0:451c27e4d55e 18 // Global //
ParinyaT 0:451c27e4d55e 19 //-- pc monitor --
ParinyaT 0:451c27e4d55e 20 Serial PC(D1,D0);
Tiz 1:9f279c68ed0c 21 //-- encoder --
ParinyaT 0:451c27e4d55e 22 int Position;
ParinyaT 5:4168693bfb80 23 int ENdata;
ParinyaT 0:451c27e4d55e 24 SPI device(Emosi, Emiso, Esck);
ParinyaT 0:451c27e4d55e 25 DigitalOut Encoder(EncoderA);
ParinyaT 0:451c27e4d55e 26 //-- Motor --
ParinyaT 5:4168693bfb80 27 int dir;
ParinyaT 0:451c27e4d55e 28 Motor LeftUpper(PWM_LU,A_LU,B_LU);
ParinyaT 5:4168693bfb80 29 //-- Memory --
ParinyaT 5:4168693bfb80 30 EEPROM memory(PB_4,PA_8,0);
ParinyaT 0:451c27e4d55e 31 //-- PID --
ParinyaT 0:451c27e4d55e 32 int SetPoint;
ParinyaT 5:4168693bfb80 33 //-- Packet --
ParinyaT 5:4168693bfb80 34 int Start1,Start2,ID,Lenght,inst,GOAL,Pos;
ParinyaT 5:4168693bfb80 35 int8_t Check,Checksum;
ParinyaT 0:451c27e4d55e 36 //*****************************************************/
ParinyaT 0:451c27e4d55e 37 void Read_Encoder()
ParinyaT 0:451c27e4d55e 38 {
ParinyaT 0:451c27e4d55e 39 SPI device(Emosi, Emiso, Esck);
ParinyaT 0:451c27e4d55e 40 device.format(8,0);
ParinyaT 0:451c27e4d55e 41 device.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
Tiz 1:9f279c68ed0c 42
ParinyaT 0:451c27e4d55e 43 Encoder = 0;
ParinyaT 0:451c27e4d55e 44 wait_us(50);
ParinyaT 0:451c27e4d55e 45 device.write(0x41);
ParinyaT 0:451c27e4d55e 46 wait_us(50);
ParinyaT 0:451c27e4d55e 47 device.write(0x09);
ParinyaT 0:451c27e4d55e 48 wait_us(50);
ParinyaT 5:4168693bfb80 49 ENdata = device.write(0x00);
ParinyaT 0:451c27e4d55e 50 wait_us(50);
ParinyaT 0:451c27e4d55e 51 Encoder = 1;
Tiz 1:9f279c68ed0c 52
ParinyaT 0:451c27e4d55e 53 }
ParinyaT 0:451c27e4d55e 54 //*****************************************************/
ParinyaT 0:451c27e4d55e 55 void Get_EnValue(int Val)
ParinyaT 0:451c27e4d55e 56 {
ParinyaT 0:451c27e4d55e 57 int i = 0;
ParinyaT 0:451c27e4d55e 58 static unsigned char codes[] = {
Tiz 1:9f279c68ed0c 59 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
Tiz 1:9f279c68ed0c 60 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
Tiz 1:9f279c68ed0c 61 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
Tiz 1:9f279c68ed0c 62 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
Tiz 1:9f279c68ed0c 63 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
Tiz 1:9f279c68ed0c 64 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
Tiz 1:9f279c68ed0c 65 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
Tiz 1:9f279c68ed0c 66 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
Tiz 1:9f279c68ed0c 67 };
Tiz 1:9f279c68ed0c 68
Tiz 1:9f279c68ed0c 69 while(i<=127) {
Tiz 1:9f279c68ed0c 70 if(Val == codes[i]) {
ParinyaT 0:451c27e4d55e 71 Position = i;
ParinyaT 0:451c27e4d55e 72 break;
Tiz 1:9f279c68ed0c 73 } else i++;
Tiz 1:9f279c68ed0c 74 }
Tiz 1:9f279c68ed0c 75 }
Tiz 1:9f279c68ed0c 76 //******************************************************************************************
Tiz 1:9f279c68ed0c 77 void eewrite()
Tiz 1:9f279c68ed0c 78 {
ParinyaT 5:4168693bfb80 79 memory.write(0x11,Kc);
ParinyaT 5:4168693bfb80 80 wait_ms(10);
ParinyaT 5:4168693bfb80 81 memory.write(0x22,Ki);
ParinyaT 5:4168693bfb80 82 wait_ms(10);
ParinyaT 5:4168693bfb80 83 memory.write(0x33,Kd);
ParinyaT 5:4168693bfb80 84
Tiz 1:9f279c68ed0c 85 wait_ms(10);
Tiz 1:9f279c68ed0c 86 }
Tiz 1:9f279c68ed0c 87 //******************************************************************************************
Tiz 1:9f279c68ed0c 88 void eeread()
Tiz 1:9f279c68ed0c 89 {
ParinyaT 5:4168693bfb80 90 memory.read(0x11,Kc);
ParinyaT 5:4168693bfb80 91 wait_ms(10);
ParinyaT 5:4168693bfb80 92 memory.read(0x22,Ki);
ParinyaT 5:4168693bfb80 93 wait_ms(10);
ParinyaT 5:4168693bfb80 94 memory.read(0x33,Kd);
Tiz 1:9f279c68ed0c 95
Tiz 1:9f279c68ed0c 96 wait_ms(10);
Tiz 1:9f279c68ed0c 97 }
Tiz 1:9f279c68ed0c 98 //******************************************************************************************
Tiz 1:9f279c68ed0c 99 void StartWriteRead(int mod)
Tiz 1:9f279c68ed0c 100 {
ParinyaT 5:4168693bfb80 101
ParinyaT 5:4168693bfb80 102 if(mod==1) {
ParinyaT 5:4168693bfb80 103 eewrite();
ParinyaT 5:4168693bfb80 104 wait_ms(10);
ParinyaT 5:4168693bfb80 105 } else if(mod==0) {
ParinyaT 5:4168693bfb80 106 eeread();
ParinyaT 5:4168693bfb80 107 wait_ms(10);
ParinyaT 5:4168693bfb80 108 }
ParinyaT 5:4168693bfb80 109
ParinyaT 0:451c27e4d55e 110 }
ParinyaT 0:451c27e4d55e 111 //*****************************************************/
ParinyaT 0:451c27e4d55e 112 int main()
ParinyaT 0:451c27e4d55e 113 {
Tiz 1:9f279c68ed0c 114
Tiz 1:9f279c68ed0c 115 uint8_t state_menu=0;
Tiz 1:9f279c68ed0c 116 uint8_t state_show=0;
Tiz 1:9f279c68ed0c 117 uint8_t state_exit =0;
Tiz 1:9f279c68ed0c 118 uint8_t data;
Tiz 1:9f279c68ed0c 119
Tiz 1:9f279c68ed0c 120 // myled = 0;
Tiz 1:9f279c68ed0c 121 PC.printf("Wellcome !\n");
Tiz 1:9f279c68ed0c 122 while(1) {
Tiz 1:9f279c68ed0c 123 if(state_show == 0) {
Tiz 1:9f279c68ed0c 124 PC.printf("Menu\n");
ParinyaT 5:4168693bfb80 125 PC.printf("1.Set PID Parameter \n");
ParinyaT 5:4168693bfb80 126 PC.printf("2.Set Position \n");
ParinyaT 5:4168693bfb80 127 PC.printf("3.PID Test \n");
Tiz 1:9f279c68ed0c 128 state_show =1;
Tiz 1:9f279c68ed0c 129 }
Tiz 1:9f279c68ed0c 130 if(PC.readable()) {
Tiz 1:9f279c68ed0c 131 data = PC.getc();
Tiz 1:9f279c68ed0c 132 PC.printf("\n");
Tiz 1:9f279c68ed0c 133 state_show =0;
Tiz 1:9f279c68ed0c 134 state_exit =0;
Tiz 1:9f279c68ed0c 135
Tiz 1:9f279c68ed0c 136 switch(data) {
Tiz 1:9f279c68ed0c 137 case '1':
Tiz 1:9f279c68ed0c 138 do {
ParinyaT 5:4168693bfb80 139
Tiz 1:9f279c68ed0c 140 if(state_menu == 0) {
ParinyaT 5:4168693bfb80 141 PC.printf("Set PID Parameter\n");
ParinyaT 5:4168693bfb80 142 PC.printf("Kp : %f\n", Kc);
ParinyaT 5:4168693bfb80 143 PC.printf("Ki : %f\n", Kc*Ti);
ParinyaT 5:4168693bfb80 144 PC.printf("Kd : %f\n", Kc*Td);
ParinyaT 5:4168693bfb80 145 PC.printf("1.New setting\n");
ParinyaT 5:4168693bfb80 146 PC.printf("2.Load setting\n");
ParinyaT 5:4168693bfb80 147 PC.printf("3.Exit\n");
Tiz 1:9f279c68ed0c 148 state_menu = 1;
Tiz 1:9f279c68ed0c 149 }
Tiz 1:9f279c68ed0c 150 if(PC.readable()) {
Tiz 1:9f279c68ed0c 151 data = PC.getc();
Tiz 1:9f279c68ed0c 152 PC.printf("\n");
Tiz 1:9f279c68ed0c 153 state_menu=0;
Tiz 1:9f279c68ed0c 154
Tiz 1:9f279c68ed0c 155 switch(data) {
Tiz 1:9f279c68ed0c 156
ParinyaT 5:4168693bfb80 157 case '1':
ParinyaT 5:4168693bfb80 158 PC.printf("Set Kp : ");
ParinyaT 5:4168693bfb80 159 PC.scanf("%f", &Kc);
ParinyaT 5:4168693bfb80 160 PC.printf("\nSet Ki : ");
ParinyaT 5:4168693bfb80 161 PC.scanf("%f", &Ki);
ParinyaT 5:4168693bfb80 162 PC.printf("\nSet Kd : ");
ParinyaT 5:4168693bfb80 163 PC.scanf("%f", &Kd);
ParinyaT 5:4168693bfb80 164
ParinyaT 5:4168693bfb80 165 Ti = Ki/Kc;
ParinyaT 5:4168693bfb80 166 Td = Kd/Kc;
ParinyaT 5:4168693bfb80 167
ParinyaT 5:4168693bfb80 168 eewrite();
ParinyaT 5:4168693bfb80 169
Tiz 1:9f279c68ed0c 170 break;
Tiz 1:9f279c68ed0c 171
ParinyaT 5:4168693bfb80 172 case '2':
ParinyaT 5:4168693bfb80 173 eeread();
ParinyaT 5:4168693bfb80 174
ParinyaT 5:4168693bfb80 175 /*PC.printf("Set Kp : %f\n",Kc);
ParinyaT 5:4168693bfb80 176 PC.printf("Set Ki : %f\n",Ki);
ParinyaT 5:4168693bfb80 177 PC.printf("Set Kd : %f\n",Kd);*/
ParinyaT 5:4168693bfb80 178
ParinyaT 5:4168693bfb80 179 Ti = Ki/Kc;
ParinyaT 5:4168693bfb80 180 Td = Kd/Kc;
ParinyaT 5:4168693bfb80 181
Tiz 1:9f279c68ed0c 182 break;
Tiz 1:9f279c68ed0c 183
ParinyaT 5:4168693bfb80 184 case '3':
ParinyaT 5:4168693bfb80 185 state_exit = 1;
Tiz 1:9f279c68ed0c 186 break;
Tiz 1:9f279c68ed0c 187
ParinyaT 5:4168693bfb80 188 //default:
ParinyaT 5:4168693bfb80 189 //PC.printf("plz select 1,2,3\n");
ParinyaT 5:4168693bfb80 190 //PC.printf("\n\n");
ParinyaT 5:4168693bfb80 191 //break;
ParinyaT 5:4168693bfb80 192
Tiz 1:9f279c68ed0c 193 }
Tiz 1:9f279c68ed0c 194 }
Tiz 1:9f279c68ed0c 195 } while(state_exit ==0);
Tiz 1:9f279c68ed0c 196 PC.printf("\n\n");
Tiz 1:9f279c68ed0c 197 break;
Tiz 1:9f279c68ed0c 198
ParinyaT 5:4168693bfb80 199 case '3':
Tiz 1:9f279c68ed0c 200
Tiz 1:9f279c68ed0c 201 do {
Tiz 1:9f279c68ed0c 202 if(state_menu == 0) {
ParinyaT 5:4168693bfb80 203 PC.printf("Mode POP\n");
ParinyaT 5:4168693bfb80 204 PC.printf("press x to exit\n\n");
ParinyaT 5:4168693bfb80 205 PID LU_PID(Kc, Ti, Td, Rate);//Kp,Ki,Kd,Rate
Tiz 1:9f279c68ed0c 206 //***start mode
ParinyaT 5:4168693bfb80 207 wait (2);
ParinyaT 5:4168693bfb80 208 while(1){
ParinyaT 5:4168693bfb80 209
Tiz 1:9f279c68ed0c 210 LeftUpper.period(0.00005);
Tiz 1:9f279c68ed0c 211 LU_PID.setInputLimits(0,127);
ParinyaT 5:4168693bfb80 212 LU_PID.setOutputLimits(0.2,1);
Tiz 1:9f279c68ed0c 213 LU_PID.setMode(AUTO_MODE);
Tiz 1:9f279c68ed0c 214
Tiz 1:9f279c68ed0c 215 //get the target position
ParinyaT 5:4168693bfb80 216 //SetPoint = 64;
Tiz 1:9f279c68ed0c 217 LU_PID.setSetPoint(SetPoint);
Tiz 1:9f279c68ed0c 218
Tiz 1:9f279c68ed0c 219 Read_Encoder();
ParinyaT 5:4168693bfb80 220 //PC.printf("%d\n",ENdata);
ParinyaT 5:4168693bfb80 221 Get_EnValue(ENdata);
ParinyaT 5:4168693bfb80 222 //PC.printf("%d\n********************\n",Position);
Tiz 1:9f279c68ed0c 223
ParinyaT 5:4168693bfb80 224 while(abs(Position-SetPoint)>1) {
Tiz 1:9f279c68ed0c 225 LU_PID.setProcessValue(Position);
Tiz 1:9f279c68ed0c 226
ParinyaT 5:4168693bfb80 227 if( Position - SetPoint > 0 ) {
ParinyaT 5:4168693bfb80 228 dir = 1;
ParinyaT 5:4168693bfb80 229 }if(Position - SetPoint < 0 ) {
ParinyaT 5:4168693bfb80 230 dir = -1;
ParinyaT 5:4168693bfb80 231 }
ParinyaT 5:4168693bfb80 232
ParinyaT 5:4168693bfb80 233
ParinyaT 5:4168693bfb80 234 LeftUpper.speed(LU_PID.compute() * dir);
ParinyaT 5:4168693bfb80 235 wait_ms(17);
Tiz 1:9f279c68ed0c 236 Read_Encoder();
ParinyaT 5:4168693bfb80 237 Get_EnValue(ENdata);
ParinyaT 5:4168693bfb80 238 //PC.printf("%d\n********************\n",Position);
ParinyaT 5:4168693bfb80 239 LeftUpper.brake(0);
ParinyaT 5:4168693bfb80 240
ParinyaT 5:4168693bfb80 241 }LeftUpper.brake(1);
ParinyaT 5:4168693bfb80 242 if(PC.readable()) {
ParinyaT 5:4168693bfb80 243 data = PC.getc();
ParinyaT 5:4168693bfb80 244 if (data == 'x') {
ParinyaT 5:4168693bfb80 245 state_exit = 1;
ParinyaT 5:4168693bfb80 246 break;
ParinyaT 5:4168693bfb80 247 }else state_menu = 0;
Tiz 1:9f279c68ed0c 248 }
ParinyaT 5:4168693bfb80 249
Tiz 1:9f279c68ed0c 250 }
ParinyaT 5:4168693bfb80 251
ParinyaT 5:4168693bfb80 252 }
Tiz 1:9f279c68ed0c 253
ParinyaT 5:4168693bfb80 254 } while(state_exit ==0);
ParinyaT 5:4168693bfb80 255 PC.printf("\n\n");
ParinyaT 5:4168693bfb80 256 break;
Tiz 1:9f279c68ed0c 257
ParinyaT 5:4168693bfb80 258 case '2':
ParinyaT 5:4168693bfb80 259 do {
Tiz 4:6e29193d7f95 260 if(state_menu == 0) {
Tiz 4:6e29193d7f95 261 PC.printf("Mode SetPoint\n");
ParinyaT 5:4168693bfb80 262 PC.printf("Waiting for command\n");
Tiz 4:6e29193d7f95 263 state_menu = 1;
Tiz 4:6e29193d7f95 264 }
ParinyaT 5:4168693bfb80 265
ParinyaT 5:4168693bfb80 266 PC.scanf("%x %x %x %x %x %x %x %x",&Start1,&Start2,&ID,&Lenght,&inst,&GOAL,&Pos,&Checksum);
ParinyaT 5:4168693bfb80 267 PC.printf("%2x %2x %2x %2x %2x %2x %2x %2x\n",Start1,Start2,ID,Lenght,inst,GOAL,Pos,Checksum);
ParinyaT 5:4168693bfb80 268 Check = ~(ID+Lenght+inst+GOAL+Pos);
ParinyaT 5:4168693bfb80 269
ParinyaT 5:4168693bfb80 270 if(inst == 03){
ParinyaT 5:4168693bfb80 271 if(GOAL == 01){
ParinyaT 5:4168693bfb80 272 if(Checksum == Check) {
ParinyaT 5:4168693bfb80 273 SetPoint=Pos;
ParinyaT 5:4168693bfb80 274 printf("Position was set to %d\n",Pos);
ParinyaT 5:4168693bfb80 275 }else printf("Worng Command\n");
Tiz 4:6e29193d7f95 276 }
Tiz 4:6e29193d7f95 277 }
ParinyaT 5:4168693bfb80 278 state_menu = 0;
ParinyaT 5:4168693bfb80 279 break;
Tiz 1:9f279c68ed0c 280
Tiz 1:9f279c68ed0c 281
Tiz 4:6e29193d7f95 282
Tiz 4:6e29193d7f95 283 } while(state_exit ==0);
Tiz 4:6e29193d7f95 284 PC.printf("\n\n");
Tiz 4:6e29193d7f95 285 break;
Tiz 1:9f279c68ed0c 286 case 0x00:
Tiz 1:9f279c68ed0c 287
Tiz 1:9f279c68ed0c 288 break;
Tiz 1:9f279c68ed0c 289
Tiz 1:9f279c68ed0c 290 default:
Tiz 4:6e29193d7f95 291 PC.printf("plz select 1 or 2 or 3 only\n");
Tiz 1:9f279c68ed0c 292 PC.printf("\n\n");
Tiz 1:9f279c68ed0c 293 break;
Tiz 1:9f279c68ed0c 294 }
Tiz 1:9f279c68ed0c 295 }
Tiz 1:9f279c68ed0c 296
Tiz 1:9f279c68ed0c 297 }
Tiz 1:9f279c68ed0c 298
Tiz 1:9f279c68ed0c 299
Tiz 1:9f279c68ed0c 300 }
Tiz 1:9f279c68ed0c 301