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@6:98871feebea0, 2016-01-16 (annotated)

Committer:
b0ssiz
Date:
Sat Jan 16 01:17:21 2016 +0000
Revision:
6:98871feebea0
Parent:
1:84167ca00307
Child:
7:bf239d051e8c
Update Bear_Receiver library

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 {
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
b0ssiz 6:98871feebea0 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;
b0ssiz 6:98871feebea0 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[] = {
b0ssiz 6:98871feebea0 60 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
b0ssiz 6:98871feebea0 61 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
b0ssiz 6:98871feebea0 62 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
b0ssiz 6:98871feebea0 63 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
b0ssiz 6:98871feebea0 64 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
b0ssiz 6:98871feebea0 65 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
b0ssiz 6:98871feebea0 66 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
b0ssiz 6:98871feebea0 67 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
b0ssiz 6:98871feebea0 68 };
b0ssiz 6:98871feebea0 69
b0ssiz 6:98871feebea0 70 while(Val != codes[i]) {
ParinyaT 1:84167ca00307 71 i++;
ParinyaT 0:451c27e4d55e 72 }
b0ssiz 6:98871feebea0 73
ParinyaT 1:84167ca00307 74 return i;
b0ssiz 6:98871feebea0 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 }
b0ssiz 6:98871feebea0 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);
b0ssiz 6:98871feebea0 100
b0ssiz 6:98871feebea0 101 if(Upper_Position - Uppper_SetPoint > 0 ) {
ParinyaT 1:84167ca00307 102 dir = 1;
b0ssiz 6:98871feebea0 103 }
b0ssiz 6:98871feebea0 104 if(Upper_Position - Uppper_SetPoint < 0 ) {
ParinyaT 1:84167ca00307 105 dir = -1;
b0ssiz 6:98871feebea0 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);
b0ssiz 6:98871feebea0 116
b0ssiz 6:98871feebea0 117 if(Lower_Position - Lower_SetPoint > 0 ) {
ParinyaT 1:84167ca00307 118 dir = 1;
b0ssiz 6:98871feebea0 119 }
b0ssiz 6:98871feebea0 120 if(Lower_Position - Lower_SetPoint < 0 ) {
ParinyaT 1:84167ca00307 121 dir = -1;
b0ssiz 6:98871feebea0 122 }
ParinyaT 1:84167ca00307 123 Lower.speed(LU_PID.compute() * dir);
b0ssiz 6:98871feebea0 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();
b0ssiz 6:98871feebea0 132
b0ssiz 6:98871feebea0 133 while(1) {
ParinyaT 1:84167ca00307 134 Up_PID.setSetPoint(Upper_SetPoint);
ParinyaT 1:84167ca00307 135 Low_PID.setSetPoint(Lower_SetPoint);
ParinyaT 1:84167ca00307 136
ParinyaT 1:84167ca00307 137 Move_Upper();
ParinyaT 1:84167ca00307 138 Move_Lower();
ParinyaT 0:451c27e4d55e 139 }
ParinyaT 1:84167ca00307 140
ParinyaT 0:451c27e4d55e 141 }
b0ssiz 6:98871feebea0 142
ParinyaT 0:451c27e4d55e 143
ParinyaT 0:451c27e4d55e 144
b0ssiz 6:98871feebea0 145
b0ssiz 6:98871feebea0 146
b0ssiz 6:98871feebea0 147