Secret_Betago / Mbed 2 deprecated BEAR_Motion_Editing4Betago

test

Dependencies:   BEAR_Reciever Motor eeprom iSerial mbed

Fork of BEAR_Motion by BE@R lab

main.cpp@7:bf239d051e8c, 2016-01-16 (annotated)

Committer:
soulx
Date:
Sat Jan 16 03:17:33 2016 +0000
Revision:
7:bf239d051e8c
Parent:
6:98871feebea0
Child:
10:3b3d6bc88677
edit for compile :Done

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ParinyaT 0:451c27e4d55e 1 //*****************************************************/
b0ssiz 6:98871feebea0 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"
b0ssiz 6:98871feebea0 8 #include "Receiver.h"
ParinyaT 0:451c27e4d55e 9
ParinyaT 0:451c27e4d55e 10 //*****************************************************/
ParinyaT 1:84167ca00307 11 //--PID parameter--
ParinyaT 1:84167ca00307 12 //-Upper-//
ParinyaT 1:84167ca00307 13 int U_Kc;
ParinyaT 1:84167ca00307 14 int U_Ti;
ParinyaT 1:84167ca00307 15 int U_Td;
ParinyaT 1:84167ca00307 16 //-lower-//
ParinyaT 1:84167ca00307 17 int L_Kc;
ParinyaT 1:84167ca00307 18 int L_Ti;
ParinyaT 1:84167ca00307 19 int L_Td;
ParinyaT 0:451c27e4d55e 20
ParinyaT 0:451c27e4d55e 21 //*****************************************************/
ParinyaT 0:451c27e4d55e 22 // Global //
ParinyaT 1:84167ca00307 23 //-- Communication --
ParinyaT 0:451c27e4d55e 24 Serial PC(D1,D0);
ParinyaT 1:84167ca00307 25 ANDANTE_PROTOCOL_PACKET command;
b0ssiz 6:98871feebea0 26 //-- encoder --
ParinyaT 1:84167ca00307 27 int Upper_Position;
ParinyaT 1:84167ca00307 28 int Lower_Position;
ParinyaT 0:451c27e4d55e 29 int data;
ParinyaT 1:84167ca00307 30 SPI ENC(Emosi, Emiso, Esck);
ParinyaT 1:84167ca00307 31 DigitalOut EncA(EncoderA);
ParinyaT 1:84167ca00307 32 DigitalOut EncB(EncoderB);
ParinyaT 0:451c27e4d55e 33 //-- Motor --
ParinyaT 1:84167ca00307 34 int dir;
ParinyaT 1:84167ca00307 35 Motor Upper(PWM_LU,A_LU,B_LU);
ParinyaT 1:84167ca00307 36 Motor Lower(PWM_LL,A_LL,B_LL);
ParinyaT 0:451c27e4d55e 37 //-- PID --
ParinyaT 1:84167ca00307 38 int Upper_SetPoint;
ParinyaT 1:84167ca00307 39 int Lower_SetPoint;
ParinyaT 1:84167ca00307 40 PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
ParinyaT 1:84167ca00307 41 PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
ParinyaT 0:451c27e4d55e 42 //*****************************************************/
ParinyaT 1:84167ca00307 43 void Read_Encoder(PinName Encoder)
ParinyaT 0:451c27e4d55e 44 {
soulx 7:bf239d051e8c 45 ENC.format(8,0);
soulx 7:bf239d051e8c 46 ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
b0ssiz 6:98871feebea0 47
soulx 7:bf239d051e8c 48 if(Encoder == EncoderA) {
soulx 7:bf239d051e8c 49 EncA = 0;
soulx 7:bf239d051e8c 50 } else {
soulx 7:bf239d051e8c 51 EncB = 0;
soulx 7:bf239d051e8c 52 }
soulx 7:bf239d051e8c 53 ENC.write(0x41);
soulx 7:bf239d051e8c 54 ENC.write(0x09);
soulx 7:bf239d051e8c 55 data = ENC.write(0x00);
soulx 7:bf239d051e8c 56 if(Encoder == EncoderA) {
soulx 7:bf239d051e8c 57 EncA = 1;
soulx 7:bf239d051e8c 58 } else {
soulx 7:bf239d051e8c 59 EncB = 1;
soulx 7:bf239d051e8c 60 }
b0ssiz 6:98871feebea0 61
ParinyaT 0:451c27e4d55e 62 }
ParinyaT 0:451c27e4d55e 63 //*****************************************************/
ParinyaT 1:84167ca00307 64 int Get_EnValue(int Val)
ParinyaT 0:451c27e4d55e 65 {
ParinyaT 0:451c27e4d55e 66 int i = 0;
ParinyaT 0:451c27e4d55e 67 static unsigned char codes[] = {
b0ssiz 6:98871feebea0 68 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
b0ssiz 6:98871feebea0 69 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
b0ssiz 6:98871feebea0 70 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
b0ssiz 6:98871feebea0 71 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
b0ssiz 6:98871feebea0 72 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
b0ssiz 6:98871feebea0 73 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
b0ssiz 6:98871feebea0 74 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
b0ssiz 6:98871feebea0 75 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
b0ssiz 6:98871feebea0 76 };
b0ssiz 6:98871feebea0 77
b0ssiz 6:98871feebea0 78 while(Val != codes[i]) {
ParinyaT 1:84167ca00307 79 i++;
ParinyaT 0:451c27e4d55e 80 }
b0ssiz 6:98871feebea0 81
ParinyaT 1:84167ca00307 82 return i;
b0ssiz 6:98871feebea0 83
ParinyaT 0:451c27e4d55e 84 }
ParinyaT 0:451c27e4d55e 85 //*****************************************************/
ParinyaT 1:84167ca00307 86 void SET_UpperPID()
ParinyaT 1:84167ca00307 87 {
ParinyaT 1:84167ca00307 88 Upper.period(0.001);
ParinyaT 1:84167ca00307 89 Up_PID.setMode(0);
ParinyaT 1:84167ca00307 90 Up_PID.setInputLimits(0,127);
ParinyaT 1:84167ca00307 91 Up_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 92 }
ParinyaT 1:84167ca00307 93 //******************************************************/
ParinyaT 1:84167ca00307 94 void SET_LowerPID()
ParinyaT 1:84167ca00307 95 {
ParinyaT 1:84167ca00307 96 Lower.period(0.001);
ParinyaT 1:84167ca00307 97 Low_PID.setMode(0);
ParinyaT 1:84167ca00307 98 Low_PID.setInputLimits(0,127);
ParinyaT 1:84167ca00307 99 Low_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 100 }
b0ssiz 6:98871feebea0 101 //******************************************************/
ParinyaT 1:84167ca00307 102 void Move_Upper()
ParinyaT 1:84167ca00307 103 {
ParinyaT 1:84167ca00307 104 Read_Encoder(EncoderA);
ParinyaT 1:84167ca00307 105 Upper_Position = Get_EnValue(data);
ParinyaT 1:84167ca00307 106
ParinyaT 1:84167ca00307 107 Up_PID.setProcessValue(Upper_Position);
b0ssiz 6:98871feebea0 108
soulx 7:bf239d051e8c 109 if(Upper_Position - Upper_SetPoint > 0 ) {
ParinyaT 1:84167ca00307 110 dir = 1;
b0ssiz 6:98871feebea0 111 }
soulx 7:bf239d051e8c 112 if(Upper_Position - Upper_SetPoint < 0 ) {
ParinyaT 1:84167ca00307 113 dir = -1;
b0ssiz 6:98871feebea0 114 }
soulx 7:bf239d051e8c 115 Upper.speed(Up_PID.compute() * dir);
ParinyaT 1:84167ca00307 116 }
ParinyaT 1:84167ca00307 117 //******************************************************/
ParinyaT 1:84167ca00307 118 void Move_Lower()
ParinyaT 1:84167ca00307 119 {
ParinyaT 1:84167ca00307 120 Read_Encoder(EncoderB);
ParinyaT 1:84167ca00307 121 Lower_Position = Get_EnValue(data);
ParinyaT 1:84167ca00307 122
ParinyaT 1:84167ca00307 123 Low_PID.setProcessValue(Lower_Position);
b0ssiz 6:98871feebea0 124
b0ssiz 6:98871feebea0 125 if(Lower_Position - Lower_SetPoint > 0 ) {
ParinyaT 1:84167ca00307 126 dir = 1;
b0ssiz 6:98871feebea0 127 }
b0ssiz 6:98871feebea0 128 if(Lower_Position - Lower_SetPoint < 0 ) {
ParinyaT 1:84167ca00307 129 dir = -1;
b0ssiz 6:98871feebea0 130 }
soulx 7:bf239d051e8c 131 Lower.speed(Low_PID.compute() * dir);
b0ssiz 6:98871feebea0 132 }
ParinyaT 1:84167ca00307 133 //******************************************************/
ParinyaT 1:84167ca00307 134
ParinyaT 1:84167ca00307 135
ParinyaT 0:451c27e4d55e 136 int main()
ParinyaT 0:451c27e4d55e 137 {
ParinyaT 1:84167ca00307 138 SET_UpperPID();
ParinyaT 1:84167ca00307 139 SET_LowerPID();
b0ssiz 6:98871feebea0 140
b0ssiz 6:98871feebea0 141 while(1) {
ParinyaT 1:84167ca00307 142 Up_PID.setSetPoint(Upper_SetPoint);
ParinyaT 1:84167ca00307 143 Low_PID.setSetPoint(Lower_SetPoint);
ParinyaT 1:84167ca00307 144
ParinyaT 1:84167ca00307 145 Move_Upper();
ParinyaT 1:84167ca00307 146 Move_Lower();
ParinyaT 0:451c27e4d55e 147 }
ParinyaT 1:84167ca00307 148
ParinyaT 0:451c27e4d55e 149 }
b0ssiz 6:98871feebea0 150
ParinyaT 0:451c27e4d55e 151
ParinyaT 0:451c27e4d55e 152
b0ssiz 6:98871feebea0 153
b0ssiz 6:98871feebea0 154
b0ssiz 6:98871feebea0 155