Secret_Betago / Mbed 2 deprecated Low_Level

first

Dependencies:   BEAR_Reciever Motor eeprom iSerial mbed

Fork of BEAR_Motion by BE@R lab

main.cpp@1:84167ca00307, 2016-01-14 (annotated)

Committer:
ParinyaT
Date:
Thu Jan 14 18:31:28 2016 +0000
Revision:
1:84167ca00307
Parent:
0:451c27e4d55e
Child:
6:98871feebea0
BEAR_Motion

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 1:84167ca00307 8 #include "Reciever.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;
ParinyaT 0:451c27e4d55e 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 {
ParinyaT 1:84167ca00307 45 Enc.format(8,0);
ParinyaT 1:84167ca00307 46 Enc.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
ParinyaT 0:451c27e4d55e 47
ParinyaT 0:451c27e4d55e 48 Encoder = 0;
ParinyaT 1:84167ca00307 49 Enc.write(0x41);
ParinyaT 1:84167ca00307 50 Enc.write(0x09);
ParinyaT 1:84167ca00307 51 data = Enc.write(0x00);
ParinyaT 0:451c27e4d55e 52 Encoder = 1;
ParinyaT 0:451c27e4d55e 53
ParinyaT 0:451c27e4d55e 54 }
ParinyaT 0:451c27e4d55e 55 //*****************************************************/
ParinyaT 1:84167ca00307 56 int Get_EnValue(int Val)
ParinyaT 0:451c27e4d55e 57 {
ParinyaT 0:451c27e4d55e 58 int i = 0;
ParinyaT 0:451c27e4d55e 59 static unsigned char codes[] = {
ParinyaT 0:451c27e4d55e 60 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
ParinyaT 0:451c27e4d55e 61 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
ParinyaT 0:451c27e4d55e 62 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
ParinyaT 0:451c27e4d55e 63 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
ParinyaT 0:451c27e4d55e 64 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
ParinyaT 0:451c27e4d55e 65 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
ParinyaT 0:451c27e4d55e 66 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
ParinyaT 0:451c27e4d55e 67 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 };
ParinyaT 0:451c27e4d55e 68
ParinyaT 1:84167ca00307 69 while(Val != codes[i])
ParinyaT 0:451c27e4d55e 70 {
ParinyaT 1:84167ca00307 71 i++;
ParinyaT 0:451c27e4d55e 72 }
ParinyaT 1:84167ca00307 73
ParinyaT 1:84167ca00307 74 return i;
ParinyaT 1:84167ca00307 75
ParinyaT 0:451c27e4d55e 76 }
ParinyaT 0:451c27e4d55e 77 //*****************************************************/
ParinyaT 1:84167ca00307 78 void SET_UpperPID()
ParinyaT 1:84167ca00307 79 {
ParinyaT 1:84167ca00307 80 Upper.period(0.001);
ParinyaT 1:84167ca00307 81 Up_PID.setMode(0);
ParinyaT 1:84167ca00307 82 Up_PID.setInputLimits(0,127);
ParinyaT 1:84167ca00307 83 Up_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 84 }
ParinyaT 1:84167ca00307 85 //******************************************************/
ParinyaT 1:84167ca00307 86 void SET_LowerPID()
ParinyaT 1:84167ca00307 87 {
ParinyaT 1:84167ca00307 88 Lower.period(0.001);
ParinyaT 1:84167ca00307 89 Low_PID.setMode(0);
ParinyaT 1:84167ca00307 90 Low_PID.setInputLimits(0,127);
ParinyaT 1:84167ca00307 91 Low_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 92 }
ParinyaT 1:84167ca00307 93 //******************************************************/
ParinyaT 1:84167ca00307 94 void Move_Upper()
ParinyaT 1:84167ca00307 95 {
ParinyaT 1:84167ca00307 96 Read_Encoder(EncoderA);
ParinyaT 1:84167ca00307 97 Upper_Position = Get_EnValue(data);
ParinyaT 1:84167ca00307 98
ParinyaT 1:84167ca00307 99 Up_PID.setProcessValue(Upper_Position);
ParinyaT 1:84167ca00307 100
ParinyaT 1:84167ca00307 101 if(Upper_Position - Uppper_SetPoint > 0 ){
ParinyaT 1:84167ca00307 102 dir = 1;
ParinyaT 1:84167ca00307 103 }
ParinyaT 1:84167ca00307 104 if(Upper_Position - Uppper_SetPoint < 0 ){
ParinyaT 1:84167ca00307 105 dir = -1;
ParinyaT 1:84167ca00307 106 }
ParinyaT 1:84167ca00307 107 Upper.speed(LU_PID.compute() * dir);
ParinyaT 1:84167ca00307 108 }
ParinyaT 1:84167ca00307 109 //******************************************************/
ParinyaT 1:84167ca00307 110 void Move_Lower()
ParinyaT 1:84167ca00307 111 {
ParinyaT 1:84167ca00307 112 Read_Encoder(EncoderB);
ParinyaT 1:84167ca00307 113 Lower_Position = Get_EnValue(data);
ParinyaT 1:84167ca00307 114
ParinyaT 1:84167ca00307 115 Low_PID.setProcessValue(Lower_Position);
ParinyaT 1:84167ca00307 116
ParinyaT 1:84167ca00307 117 if(Lower_Position - Lower_SetPoint > 0 ){
ParinyaT 1:84167ca00307 118 dir = 1;
ParinyaT 1:84167ca00307 119 }
ParinyaT 1:84167ca00307 120 if(Lower_Position - Lower_SetPoint < 0 ){
ParinyaT 1:84167ca00307 121 dir = -1;
ParinyaT 1:84167ca00307 122 }
ParinyaT 1:84167ca00307 123 Lower.speed(LU_PID.compute() * dir);
ParinyaT 1:84167ca00307 124 }
ParinyaT 1:84167ca00307 125 //******************************************************/
ParinyaT 1:84167ca00307 126
ParinyaT 1:84167ca00307 127
ParinyaT 0:451c27e4d55e 128 int main()
ParinyaT 0:451c27e4d55e 129 {
ParinyaT 1:84167ca00307 130 SET_UpperPID();
ParinyaT 1:84167ca00307 131 SET_LowerPID();
ParinyaT 0:451c27e4d55e 132
ParinyaT 1:84167ca00307 133 while(1)
ParinyaT 0:451c27e4d55e 134 {
ParinyaT 1:84167ca00307 135 Up_PID.setSetPoint(Upper_SetPoint);
ParinyaT 1:84167ca00307 136 Low_PID.setSetPoint(Lower_SetPoint);
ParinyaT 1:84167ca00307 137
ParinyaT 1:84167ca00307 138 Move_Upper();
ParinyaT 1:84167ca00307 139 Move_Lower();
ParinyaT 0:451c27e4d55e 140 }
ParinyaT 1:84167ca00307 141
ParinyaT 0:451c27e4d55e 142 }
ParinyaT 0:451c27e4d55e 143
ParinyaT 0:451c27e4d55e 144
ParinyaT 0:451c27e4d55e 145
ParinyaT 0:451c27e4d55e 146
ParinyaT 0:451c27e4d55e 147
ParinyaT 0:451c27e4d55e 148