Digital B14 / Mbed 2 deprecated Tanginamo

qwq

Dependencies:   Motor PID eeprom mbed

Fork of DogPID by Digital B14

main.cpp@4:6e29193d7f95, 2015-12-07 (annotated)

Committer:
Tiz
Date:
Mon Dec 07 21:32:46 2015 +0000
Revision:
4:6e29193d7f95
Parent:
3:6e2ad66e591c 
??????????

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