BE@R lab / Mbed 2 deprecated BEAR_Motion

โปรแกรมของบอร์ด Motion

Dependencies:   BEAR_Reciever Motor eeprom iSerial mbed

Fork of DogPID by Digital B14

main.cpp@0:451c27e4d55e, 2015-12-07 (annotated)

Committer:
ParinyaT
Date:
Mon Dec 07 14:41:42 2015 +0000
Revision:
0:451c27e4d55e
Child:
1:84167ca00307
Child:
2:9f279c68ed0c
project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ParinyaT 0:451c27e4d55e 1 //*****************************************************/
ParinyaT 0:451c27e4d55e 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 0:451c27e4d55e 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 0:451c27e4d55e 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 0:451c27e4d55e 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 0:451c27e4d55e 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 0:451c27e4d55e 53 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
ParinyaT 0:451c27e4d55e 54 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
ParinyaT 0:451c27e4d55e 55 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
ParinyaT 0:451c27e4d55e 56 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
ParinyaT 0:451c27e4d55e 57 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
ParinyaT 0:451c27e4d55e 58 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
ParinyaT 0:451c27e4d55e 59 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
ParinyaT 0:451c27e4d55e 60 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 };
ParinyaT 0:451c27e4d55e 61
ParinyaT 0:451c27e4d55e 62 while(i<=127)
ParinyaT 0:451c27e4d55e 63 {
ParinyaT 0:451c27e4d55e 64 if(Val == codes[i])
ParinyaT 0:451c27e4d55e 65 {
ParinyaT 0:451c27e4d55e 66 Position = i;
ParinyaT 0:451c27e4d55e 67 break;
ParinyaT 0:451c27e4d55e 68 }
ParinyaT 0:451c27e4d55e 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 0:451c27e4d55e 75 LeftUpper.period(0.00005);
ParinyaT 0:451c27e4d55e 76 LU_PID.setInputLimits(0,127);
ParinyaT 0:451c27e4d55e 77 LU_PID.setOutputLimits(0,0.9);
ParinyaT 0:451c27e4d55e 78 LU_PID.setMode(AUTO_MODE);
ParinyaT 0:451c27e4d55e 79
ParinyaT 0:451c27e4d55e 80 //get the target position
ParinyaT 0:451c27e4d55e 81 SetPoint = 63;
ParinyaT 0:451c27e4d55e 82 LU_PID.setSetPoint(SetPoint);
ParinyaT 0:451c27e4d55e 83
ParinyaT 0:451c27e4d55e 84 Read_Encoder();
ParinyaT 0:451c27e4d55e 85 PC.printf("%d\n",data);
ParinyaT 0:451c27e4d55e 86 Get_EnValue(data);
ParinyaT 0:451c27e4d55e 87 PC.printf("%d\n********************\n",Position);
ParinyaT 0:451c27e4d55e 88
ParinyaT 0:451c27e4d55e 89 while(Position != SetPoint)
ParinyaT 0:451c27e4d55e 90 {
ParinyaT 0:451c27e4d55e 91 LU_PID.setProcessValue(Position);
ParinyaT 0:451c27e4d55e 92 LeftUpper.speed(LU_PID.compute());
ParinyaT 0:451c27e4d55e 93
ParinyaT 0:451c27e4d55e 94 Read_Encoder();
ParinyaT 0:451c27e4d55e 95 Get_EnValue(data);
ParinyaT 0:451c27e4d55e 96 }
ParinyaT 0:451c27e4d55e 97
ParinyaT 0:451c27e4d55e 98
ParinyaT 0:451c27e4d55e 99 }
ParinyaT 0:451c27e4d55e 100
ParinyaT 0:451c27e4d55e 101
ParinyaT 0:451c27e4d55e 102
ParinyaT 0:451c27e4d55e 103
ParinyaT 0:451c27e4d55e 104
ParinyaT 0:451c27e4d55e 105