first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of BEAR_Motion by
Diff: main.cpp
- Revision:
- 0:451c27e4d55e
- Child:
- 1:84167ca00307
- Child:
- 2:9f279c68ed0c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Dec 07 14:41:42 2015 +0000 @@ -0,0 +1,105 @@ +//*****************************************************/ +// Include // +#include "mbed.h" +#include "pinconfig.h" +#include "PID.h" +#include "Motor.h" +#include "eeprom.h" + +//*****************************************************/ +// Defines // +#define Rate 0.01 +#define Kc -2.6 +#define Ti 0.0 +#define Td 0.0 + +//*****************************************************/ +// Global // +//-- pc monitor -- +Serial PC(D1,D0); +//-- encoder -- +int Position; +int data; +SPI device(Emosi, Emiso, Esck); +DigitalOut Encoder(EncoderA); +//-- Motor -- +Motor LeftUpper(PWM_LU,A_LU,B_LU); +//-- PID -- +int SetPoint; +PID LU_PID(Kc, Ti, Td, Rate);//Kp,Ki,Kd,Rate +//*****************************************************/ +void Read_Encoder() +{ + SPI device(Emosi, Emiso, Esck); + device.format(8,0); + device.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k + + Encoder = 0; + wait_us(50); + device.write(0x41); + wait_us(50); + device.write(0x09); + wait_us(50); + data = device.write(0x00); + wait_us(50); + Encoder = 1; + +} +//*****************************************************/ +void Get_EnValue(int Val) +{ + int i = 0; + static unsigned char codes[] = { + 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175, + 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215, + 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235, + 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245, + 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250, + 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125, + 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, + 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 }; + + while(i<=127) + { + if(Val == codes[i]) + { + Position = i; + break; + } + else i++; + } +} +//*****************************************************/ +int main() +{ + LeftUpper.period(0.00005); + LU_PID.setInputLimits(0,127); + LU_PID.setOutputLimits(0,0.9); + LU_PID.setMode(AUTO_MODE); + + //get the target position + SetPoint = 63; + LU_PID.setSetPoint(SetPoint); + + Read_Encoder(); + PC.printf("%d\n",data); + Get_EnValue(data); + PC.printf("%d\n********************\n",Position); + + while(Position != SetPoint) + { + LU_PID.setProcessValue(Position); + LeftUpper.speed(LU_PID.compute()); + + Read_Encoder(); + Get_EnValue(data); + } + + +} + + + + + +