![](/media/cache/group/11700697_10200701668420946_8214886085951071495_o.jpg.50x50_q85.jpg)
โปรแกรมของบอร์ด Motion
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of DogPID by
main.cpp@4:fd06f8fdec19, 2016-01-14 (annotated)
- Committer:
- ParinyaT
- Date:
- Thu Jan 14 18:44:04 2016 +0000
- Revision:
- 4:fd06f8fdec19
- Parent:
- 2:9f279c68ed0c
Motion;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ParinyaT | 0:451c27e4d55e | 1 | //*****************************************************/ |
ParinyaT | 4:fd06f8fdec19 | 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 | 4:fd06f8fdec19 | 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); |
ParinyaT | 4:fd06f8fdec19 | 20 | //-- encoder -- |
ParinyaT | 0:451c27e4d55e | 21 | int Position; |
ParinyaT | 0:451c27e4d55e | 22 | int data; |
ParinyaT | 0:451c27e4d55e | 23 | SPI device(Emosi, Emiso, Esck); |
ParinyaT | 0:451c27e4d55e | 24 | DigitalOut Encoder(EncoderA); |
ParinyaT | 0:451c27e4d55e | 25 | //-- Motor -- |
ParinyaT | 0:451c27e4d55e | 26 | Motor LeftUpper(PWM_LU,A_LU,B_LU); |
ParinyaT | 0:451c27e4d55e | 27 | //-- PID -- |
ParinyaT | 0:451c27e4d55e | 28 | int SetPoint; |
ParinyaT | 0:451c27e4d55e | 29 | PID LU_PID(Kc, Ti, Td, Rate);//Kp,Ki,Kd,Rate |
ParinyaT | 0:451c27e4d55e | 30 | //*****************************************************/ |
ParinyaT | 0:451c27e4d55e | 31 | void Read_Encoder() |
ParinyaT | 0:451c27e4d55e | 32 | { |
ParinyaT | 0:451c27e4d55e | 33 | SPI device(Emosi, Emiso, Esck); |
ParinyaT | 0:451c27e4d55e | 34 | device.format(8,0); |
ParinyaT | 0:451c27e4d55e | 35 | device.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k |
ParinyaT | 4:fd06f8fdec19 | 36 | |
ParinyaT | 0:451c27e4d55e | 37 | Encoder = 0; |
ParinyaT | 0:451c27e4d55e | 38 | wait_us(50); |
ParinyaT | 0:451c27e4d55e | 39 | device.write(0x41); |
ParinyaT | 0:451c27e4d55e | 40 | wait_us(50); |
ParinyaT | 0:451c27e4d55e | 41 | device.write(0x09); |
ParinyaT | 0:451c27e4d55e | 42 | wait_us(50); |
ParinyaT | 0:451c27e4d55e | 43 | data = device.write(0x00); |
ParinyaT | 0:451c27e4d55e | 44 | wait_us(50); |
ParinyaT | 0:451c27e4d55e | 45 | Encoder = 1; |
ParinyaT | 4:fd06f8fdec19 | 46 | |
ParinyaT | 0:451c27e4d55e | 47 | } |
ParinyaT | 0:451c27e4d55e | 48 | //*****************************************************/ |
ParinyaT | 0:451c27e4d55e | 49 | void Get_EnValue(int Val) |
ParinyaT | 0:451c27e4d55e | 50 | { |
ParinyaT | 0:451c27e4d55e | 51 | int i = 0; |
ParinyaT | 0:451c27e4d55e | 52 | static unsigned char codes[] = { |
ParinyaT | 4:fd06f8fdec19 | 53 | 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175, |
ParinyaT | 4:fd06f8fdec19 | 54 | 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215, |
ParinyaT | 4:fd06f8fdec19 | 55 | 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235, |
ParinyaT | 4:fd06f8fdec19 | 56 | 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245, |
ParinyaT | 4:fd06f8fdec19 | 57 | 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250, |
ParinyaT | 4:fd06f8fdec19 | 58 | 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125, |
ParinyaT | 4:fd06f8fdec19 | 59 | 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, |
ParinyaT | 4:fd06f8fdec19 | 60 | 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 }; |
ParinyaT | 4:fd06f8fdec19 | 61 | |
ParinyaT | 4:fd06f8fdec19 | 62 | while(i<=127) |
ParinyaT | 4:fd06f8fdec19 | 63 | { |
ParinyaT | 4:fd06f8fdec19 | 64 | if(Val == codes[i]) |
ParinyaT | 4:fd06f8fdec19 | 65 | { |
ParinyaT | 0:451c27e4d55e | 66 | Position = i; |
ParinyaT | 0:451c27e4d55e | 67 | break; |
ParinyaT | 0:451c27e4d55e | 68 | } |
ParinyaT | 4:fd06f8fdec19 | 69 | else i++; |
ParinyaT | 0:451c27e4d55e | 70 | } |
ParinyaT | 0:451c27e4d55e | 71 | } |
ParinyaT | 0:451c27e4d55e | 72 | //*****************************************************/ |
ParinyaT | 0:451c27e4d55e | 73 | int main() |
ParinyaT | 0:451c27e4d55e | 74 | { |
ParinyaT | 4:fd06f8fdec19 | 75 | LeftUpper.period(0.00005); |
ParinyaT | 4:fd06f8fdec19 | 76 | LU_PID.setInputLimits(0,127); |
ParinyaT | 4:fd06f8fdec19 | 77 | LU_PID.setOutputLimits(0,0.9); |
ParinyaT | 4:fd06f8fdec19 | 78 | LU_PID.setMode(AUTO_MODE); |
ParinyaT | 4:fd06f8fdec19 | 79 | |
ParinyaT | 4:fd06f8fdec19 | 80 | //get the target position |
ParinyaT | 4:fd06f8fdec19 | 81 | SetPoint = 63; |
ParinyaT | 4:fd06f8fdec19 | 82 | LU_PID.setSetPoint(SetPoint); |
ParinyaT | 4:fd06f8fdec19 | 83 | |
ParinyaT | 4:fd06f8fdec19 | 84 | Read_Encoder(); |
ParinyaT | 4:fd06f8fdec19 | 85 | PC.printf("%d\n",data); |
ParinyaT | 4:fd06f8fdec19 | 86 | Get_EnValue(data); |
ParinyaT | 4:fd06f8fdec19 | 87 | PC.printf("%d\n********************\n",Position); |
ParinyaT | 4:fd06f8fdec19 | 88 | |
ParinyaT | 4:fd06f8fdec19 | 89 | while(Position != SetPoint) |
ParinyaT | 4:fd06f8fdec19 | 90 | { |
ParinyaT | 4:fd06f8fdec19 | 91 | LU_PID.setProcessValue(Position); |
ParinyaT | 4:fd06f8fdec19 | 92 | LeftUpper.speed(LU_PID.compute()); |
ParinyaT | 4:fd06f8fdec19 | 93 | |
ParinyaT | 4:fd06f8fdec19 | 94 | Read_Encoder(); |
ParinyaT | 4:fd06f8fdec19 | 95 | Get_EnValue(data); |
ParinyaT | 4:fd06f8fdec19 | 96 | } |
ParinyaT | 4:fd06f8fdec19 | 97 | |
ParinyaT | 4:fd06f8fdec19 | 98 | |
ParinyaT | 4:fd06f8fdec19 | 99 | } |
ParinyaT | 4:fd06f8fdec19 | 100 | |
ParinyaT | 4:fd06f8fdec19 | 101 | |
ParinyaT | 4:fd06f8fdec19 | 102 | |
ParinyaT | 4:fd06f8fdec19 | 103 | |
ParinyaT | 0:451c27e4d55e | 104 | |
ParinyaT | 0:451c27e4d55e | 105 |