โปรแกรมของบอร์ด Motion
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of DogPID by
main.cpp@2:9f279c68ed0c, 2015-12-07 (annotated)
- Committer:
- Tiz
- Date:
- Mon Dec 07 17:09:33 2015 +0000
- Revision:
- 2:9f279c68ed0c
- Parent:
- 0:451c27e4d55e
- Child:
- 4:fd06f8fdec19
Pong Dry
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ParinyaT | 0:451c27e4d55e | 1 | //*****************************************************/ |
Tiz | 2: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 | 2: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 | 2:9f279c68ed0c | 20 | //-- encoder -- |
ParinyaT | 0:451c27e4d55e | 21 | int Position; |
ParinyaT | 0:451c27e4d55e | 22 | int data; |
Tiz | 2:9f279c68ed0c | 23 | |
Tiz | 2:9f279c68ed0c | 24 | |
Tiz | 2:9f279c68ed0c | 25 | //******************************************************* |
Tiz | 2:9f279c68ed0c | 26 | |
Tiz | 2:9f279c68ed0c | 27 | |
Tiz | 2:9f279c68ed0c | 28 | //write encoder |
Tiz | 2:9f279c68ed0c | 29 | EEPROM memory(I2C_SDA,I2C_SCL,0); |
Tiz | 2:9f279c68ed0c | 30 | int8_t out,write; |
Tiz | 2:9f279c68ed0c | 31 | int mod; |
Tiz | 2:9f279c68ed0c | 32 | InterruptIn send(D11); |
Tiz | 2:9f279c68ed0c | 33 | int FromSerial; |
Tiz | 2:9f279c68ed0c | 34 | |
Tiz | 2:9f279c68ed0c | 35 | |
Tiz | 2:9f279c68ed0c | 36 | //******************************************************* |
ParinyaT | 0:451c27e4d55e | 37 | SPI device(Emosi, Emiso, Esck); |
ParinyaT | 0:451c27e4d55e | 38 | DigitalOut Encoder(EncoderA); |
ParinyaT | 0:451c27e4d55e | 39 | //-- Motor -- |
ParinyaT | 0:451c27e4d55e | 40 | Motor LeftUpper(PWM_LU,A_LU,B_LU); |
ParinyaT | 0:451c27e4d55e | 41 | //-- PID -- |
ParinyaT | 0:451c27e4d55e | 42 | int SetPoint; |
ParinyaT | 0:451c27e4d55e | 43 | PID LU_PID(Kc, Ti, Td, Rate);//Kp,Ki,Kd,Rate |
ParinyaT | 0:451c27e4d55e | 44 | //*****************************************************/ |
ParinyaT | 0:451c27e4d55e | 45 | void Read_Encoder() |
ParinyaT | 0:451c27e4d55e | 46 | { |
ParinyaT | 0:451c27e4d55e | 47 | SPI device(Emosi, Emiso, Esck); |
ParinyaT | 0:451c27e4d55e | 48 | device.format(8,0); |
ParinyaT | 0:451c27e4d55e | 49 | device.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k |
Tiz | 2:9f279c68ed0c | 50 | |
ParinyaT | 0:451c27e4d55e | 51 | Encoder = 0; |
ParinyaT | 0:451c27e4d55e | 52 | wait_us(50); |
ParinyaT | 0:451c27e4d55e | 53 | device.write(0x41); |
ParinyaT | 0:451c27e4d55e | 54 | wait_us(50); |
ParinyaT | 0:451c27e4d55e | 55 | device.write(0x09); |
ParinyaT | 0:451c27e4d55e | 56 | wait_us(50); |
ParinyaT | 0:451c27e4d55e | 57 | data = device.write(0x00); |
ParinyaT | 0:451c27e4d55e | 58 | wait_us(50); |
ParinyaT | 0:451c27e4d55e | 59 | Encoder = 1; |
Tiz | 2:9f279c68ed0c | 60 | |
ParinyaT | 0:451c27e4d55e | 61 | } |
ParinyaT | 0:451c27e4d55e | 62 | //*****************************************************/ |
ParinyaT | 0:451c27e4d55e | 63 | void Get_EnValue(int Val) |
ParinyaT | 0:451c27e4d55e | 64 | { |
ParinyaT | 0:451c27e4d55e | 65 | int i = 0; |
ParinyaT | 0:451c27e4d55e | 66 | static unsigned char codes[] = { |
Tiz | 2:9f279c68ed0c | 67 | 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175, |
Tiz | 2:9f279c68ed0c | 68 | 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215, |
Tiz | 2:9f279c68ed0c | 69 | 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235, |
Tiz | 2:9f279c68ed0c | 70 | 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245, |
Tiz | 2:9f279c68ed0c | 71 | 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250, |
Tiz | 2:9f279c68ed0c | 72 | 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125, |
Tiz | 2:9f279c68ed0c | 73 | 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, |
Tiz | 2:9f279c68ed0c | 74 | 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 |
Tiz | 2:9f279c68ed0c | 75 | }; |
Tiz | 2:9f279c68ed0c | 76 | |
Tiz | 2:9f279c68ed0c | 77 | while(i<=127) { |
Tiz | 2:9f279c68ed0c | 78 | if(Val == codes[i]) { |
ParinyaT | 0:451c27e4d55e | 79 | Position = i; |
ParinyaT | 0:451c27e4d55e | 80 | break; |
Tiz | 2:9f279c68ed0c | 81 | } else i++; |
Tiz | 2:9f279c68ed0c | 82 | } |
Tiz | 2:9f279c68ed0c | 83 | } |
Tiz | 2:9f279c68ed0c | 84 | //****************************************************************************************** |
Tiz | 2:9f279c68ed0c | 85 | void eewrite() |
Tiz | 2:9f279c68ed0c | 86 | { |
Tiz | 2:9f279c68ed0c | 87 | write = Position; |
Tiz | 2:9f279c68ed0c | 88 | memory.write(0xFF,write); |
Tiz | 2:9f279c68ed0c | 89 | PC.printf("%x\n",write); |
Tiz | 2:9f279c68ed0c | 90 | |
Tiz | 2:9f279c68ed0c | 91 | wait_ms(10); |
Tiz | 2:9f279c68ed0c | 92 | } |
Tiz | 2:9f279c68ed0c | 93 | //****************************************************************************************** |
Tiz | 2:9f279c68ed0c | 94 | void eeread() |
Tiz | 2:9f279c68ed0c | 95 | { |
Tiz | 2:9f279c68ed0c | 96 | memory.read(0xFF,out); |
Tiz | 2:9f279c68ed0c | 97 | PC.printf("%x\n",out); |
Tiz | 2:9f279c68ed0c | 98 | |
Tiz | 2:9f279c68ed0c | 99 | wait_ms(10); |
Tiz | 2:9f279c68ed0c | 100 | } |
Tiz | 2:9f279c68ed0c | 101 | //****************************************************************************************** |
Tiz | 2:9f279c68ed0c | 102 | void StartWriteRead(int mod) |
Tiz | 2:9f279c68ed0c | 103 | { |
Tiz | 2:9f279c68ed0c | 104 | while(1) { |
Tiz | 2:9f279c68ed0c | 105 | if(mod==1) { |
Tiz | 2:9f279c68ed0c | 106 | send.rise(eewrite); |
Tiz | 2:9f279c68ed0c | 107 | wait_ms(10); |
Tiz | 2:9f279c68ed0c | 108 | } else if(mod==0) { |
Tiz | 2:9f279c68ed0c | 109 | send.rise(eeread); |
Tiz | 2:9f279c68ed0c | 110 | wait_ms(10); |
ParinyaT | 0:451c27e4d55e | 111 | } |
ParinyaT | 0:451c27e4d55e | 112 | } |
ParinyaT | 0:451c27e4d55e | 113 | } |
ParinyaT | 0:451c27e4d55e | 114 | //*****************************************************/ |
ParinyaT | 0:451c27e4d55e | 115 | int main() |
ParinyaT | 0:451c27e4d55e | 116 | { |
Tiz | 2:9f279c68ed0c | 117 | |
Tiz | 2:9f279c68ed0c | 118 | uint8_t state_menu=0; |
Tiz | 2:9f279c68ed0c | 119 | uint8_t state_show=0; |
Tiz | 2:9f279c68ed0c | 120 | uint8_t state_exit =0; |
Tiz | 2:9f279c68ed0c | 121 | uint8_t data; |
Tiz | 2:9f279c68ed0c | 122 | |
Tiz | 2:9f279c68ed0c | 123 | // myled = 0; |
Tiz | 2:9f279c68ed0c | 124 | PC.printf("Wellcome !\n"); |
Tiz | 2:9f279c68ed0c | 125 | while(1) { |
Tiz | 2:9f279c68ed0c | 126 | if(state_show == 0) { |
Tiz | 2:9f279c68ed0c | 127 | PC.printf("Menu\n"); |
Tiz | 2:9f279c68ed0c | 128 | PC.printf("1.Mode Write Or Read \n"); |
Tiz | 2:9f279c68ed0c | 129 | PC.printf("2.Logic Input Test\n"); |
Tiz | 2:9f279c68ed0c | 130 | state_show =1; |
Tiz | 2:9f279c68ed0c | 131 | } |
Tiz | 2:9f279c68ed0c | 132 | if(PC.readable()) { |
Tiz | 2:9f279c68ed0c | 133 | data = PC.getc(); |
Tiz | 2:9f279c68ed0c | 134 | PC.printf("\n"); |
Tiz | 2:9f279c68ed0c | 135 | state_show =0; |
Tiz | 2:9f279c68ed0c | 136 | state_exit =0; |
Tiz | 2:9f279c68ed0c | 137 | |
Tiz | 2:9f279c68ed0c | 138 | switch(data) { |
Tiz | 2:9f279c68ed0c | 139 | case '1': |
Tiz | 2:9f279c68ed0c | 140 | do { |
Tiz | 2:9f279c68ed0c | 141 | if(state_menu == 0) { |
Tiz | 2:9f279c68ed0c | 142 | PC.printf("Mode Write Or Read\n"); |
Tiz | 2:9f279c68ed0c | 143 | PC.printf("a.Mode Write \n"); |
Tiz | 2:9f279c68ed0c | 144 | PC.printf("d.Mode Read \n"); |
Tiz | 2:9f279c68ed0c | 145 | PC.printf("s.Stop \n"); |
Tiz | 2:9f279c68ed0c | 146 | state_menu = 1; |
Tiz | 2:9f279c68ed0c | 147 | } |
Tiz | 2:9f279c68ed0c | 148 | if(PC.readable()) { |
Tiz | 2:9f279c68ed0c | 149 | data = PC.getc(); |
Tiz | 2:9f279c68ed0c | 150 | PC.printf("\n"); |
Tiz | 2:9f279c68ed0c | 151 | state_menu=0; |
Tiz | 2:9f279c68ed0c | 152 | |
Tiz | 2:9f279c68ed0c | 153 | switch(data) { |
Tiz | 2:9f279c68ed0c | 154 | |
Tiz | 2:9f279c68ed0c | 155 | case 'a': |
Tiz | 2:9f279c68ed0c | 156 | StartWriteRead(1); |
Tiz | 2:9f279c68ed0c | 157 | break; |
Tiz | 2:9f279c68ed0c | 158 | |
Tiz | 2:9f279c68ed0c | 159 | case 's': |
Tiz | 2:9f279c68ed0c | 160 | state_exit = 1; |
Tiz | 2:9f279c68ed0c | 161 | |
Tiz | 2:9f279c68ed0c | 162 | break; |
Tiz | 2:9f279c68ed0c | 163 | |
Tiz | 2:9f279c68ed0c | 164 | case 'd': |
Tiz | 2:9f279c68ed0c | 165 | StartWriteRead(0); |
Tiz | 2:9f279c68ed0c | 166 | break; |
Tiz | 2:9f279c68ed0c | 167 | |
Tiz | 2:9f279c68ed0c | 168 | default: |
Tiz | 2:9f279c68ed0c | 169 | PC.printf("plz select a or d\n"); |
Tiz | 2:9f279c68ed0c | 170 | PC.printf("\n\n"); |
Tiz | 2:9f279c68ed0c | 171 | break; |
Tiz | 2:9f279c68ed0c | 172 | |
Tiz | 2:9f279c68ed0c | 173 | } |
Tiz | 2:9f279c68ed0c | 174 | } |
Tiz | 2:9f279c68ed0c | 175 | |
ParinyaT | 0:451c27e4d55e | 176 | |
ParinyaT | 0:451c27e4d55e | 177 | |
Tiz | 2:9f279c68ed0c | 178 | } while(state_exit ==0); |
Tiz | 2:9f279c68ed0c | 179 | PC.printf("\n\n"); |
Tiz | 2:9f279c68ed0c | 180 | break; |
Tiz | 2:9f279c68ed0c | 181 | |
Tiz | 2:9f279c68ed0c | 182 | case '2': |
Tiz | 2:9f279c68ed0c | 183 | |
Tiz | 2:9f279c68ed0c | 184 | do { |
Tiz | 2:9f279c68ed0c | 185 | if(state_menu == 0) { |
Tiz | 2:9f279c68ed0c | 186 | PC.printf("Mode POP\n\n"); |
Tiz | 2:9f279c68ed0c | 187 | //***start mode |
Tiz | 2:9f279c68ed0c | 188 | { |
Tiz | 2:9f279c68ed0c | 189 | LeftUpper.period(0.00005); |
Tiz | 2:9f279c68ed0c | 190 | LU_PID.setInputLimits(0,127); |
Tiz | 2:9f279c68ed0c | 191 | LU_PID.setOutputLimits(0,0.9); |
Tiz | 2:9f279c68ed0c | 192 | LU_PID.setMode(AUTO_MODE); |
Tiz | 2:9f279c68ed0c | 193 | |
Tiz | 2:9f279c68ed0c | 194 | //get the target position |
Tiz | 2:9f279c68ed0c | 195 | SetPoint = 63; |
Tiz | 2:9f279c68ed0c | 196 | LU_PID.setSetPoint(SetPoint); |
Tiz | 2:9f279c68ed0c | 197 | |
Tiz | 2:9f279c68ed0c | 198 | Read_Encoder(); |
Tiz | 2:9f279c68ed0c | 199 | PC.printf("%d\n",data); |
Tiz | 2:9f279c68ed0c | 200 | Get_EnValue(data); |
Tiz | 2:9f279c68ed0c | 201 | PC.printf("%d\n********************\n",Position); |
Tiz | 2:9f279c68ed0c | 202 | |
Tiz | 2:9f279c68ed0c | 203 | while(Position != SetPoint) { |
Tiz | 2:9f279c68ed0c | 204 | LU_PID.setProcessValue(Position); |
Tiz | 2:9f279c68ed0c | 205 | LeftUpper.speed(LU_PID.compute()); |
Tiz | 2:9f279c68ed0c | 206 | |
Tiz | 2:9f279c68ed0c | 207 | Read_Encoder(); |
Tiz | 2:9f279c68ed0c | 208 | Get_EnValue(data); |
Tiz | 2:9f279c68ed0c | 209 | } |
Tiz | 2:9f279c68ed0c | 210 | } |
Tiz | 2:9f279c68ed0c | 211 | PC.printf("press x to exit or else to continue\n"); |
Tiz | 2:9f279c68ed0c | 212 | state_menu = 1; |
Tiz | 2:9f279c68ed0c | 213 | } |
Tiz | 2:9f279c68ed0c | 214 | if(PC.readable()) { |
Tiz | 2:9f279c68ed0c | 215 | data = PC.getc(); |
Tiz | 2:9f279c68ed0c | 216 | if (data == 'x') state_exit = 1; |
Tiz | 2:9f279c68ed0c | 217 | else state_menu = 0; |
Tiz | 2:9f279c68ed0c | 218 | } |
Tiz | 2:9f279c68ed0c | 219 | |
Tiz | 2:9f279c68ed0c | 220 | |
Tiz | 2:9f279c68ed0c | 221 | |
Tiz | 2:9f279c68ed0c | 222 | } while(state_exit ==0); |
Tiz | 2:9f279c68ed0c | 223 | PC.printf("\n\n"); |
Tiz | 2:9f279c68ed0c | 224 | break; |
Tiz | 2:9f279c68ed0c | 225 | |
Tiz | 2:9f279c68ed0c | 226 | |
Tiz | 2:9f279c68ed0c | 227 | case 0x00: |
Tiz | 2:9f279c68ed0c | 228 | |
Tiz | 2:9f279c68ed0c | 229 | break; |
Tiz | 2:9f279c68ed0c | 230 | |
Tiz | 2:9f279c68ed0c | 231 | default: |
Tiz | 2:9f279c68ed0c | 232 | PC.printf("plz select 1 or 2 only\n"); |
Tiz | 2:9f279c68ed0c | 233 | PC.printf("\n\n"); |
Tiz | 2:9f279c68ed0c | 234 | break; |
Tiz | 2:9f279c68ed0c | 235 | } |
Tiz | 2:9f279c68ed0c | 236 | } |
Tiz | 2:9f279c68ed0c | 237 | |
Tiz | 2:9f279c68ed0c | 238 | } |
Tiz | 2:9f279c68ed0c | 239 | |
Tiz | 2:9f279c68ed0c | 240 | |
Tiz | 2:9f279c68ed0c | 241 | } |
Tiz | 2:9f279c68ed0c | 242 | |
Tiz | 2:9f279c68ed0c | 243 | |
Tiz | 2:9f279c68ed0c | 244 | |
Tiz | 2:9f279c68ed0c | 245 | |
Tiz | 2:9f279c68ed0c | 246 | |
Tiz | 2:9f279c68ed0c | 247 |