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@10:3b3d6bc88677, 2016-01-16 (annotated)

Committer:
b0ssiz
Date:
Sat Jan 16 12:06:59 2016 +0000
Revision:
10:3b3d6bc88677
Parent:
7:bf239d051e8c
Child:
11:3dd92d1d542c
Update receiver

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-//
b0ssiz 10:3b3d6bc88677 13 float U_Kc;
b0ssiz 10:3b3d6bc88677 14 float U_Ti;
b0ssiz 10:3b3d6bc88677 15 float U_Td;
ParinyaT 1:84167ca00307 16 //-lower-//
b0ssiz 10:3b3d6bc88677 17 float L_Kc;
b0ssiz 10:3b3d6bc88677 18 float L_Ti;
b0ssiz 10:3b3d6bc88677 19 float 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 10:3b3d6bc88677 26 Bear_Receiver com(Tx,Rx,115200);
b0ssiz 10:3b3d6bc88677 27 #define MY_ID 0x01
b0ssiz 6:98871feebea0 28 //-- encoder --
ParinyaT 1:84167ca00307 29 int Upper_Position;
ParinyaT 1:84167ca00307 30 int Lower_Position;
ParinyaT 0:451c27e4d55e 31 int data;
ParinyaT 1:84167ca00307 32 SPI ENC(Emosi, Emiso, Esck);
ParinyaT 1:84167ca00307 33 DigitalOut EncA(EncoderA);
ParinyaT 1:84167ca00307 34 DigitalOut EncB(EncoderB);
ParinyaT 0:451c27e4d55e 35 //-- Motor --
ParinyaT 1:84167ca00307 36 int dir;
ParinyaT 1:84167ca00307 37 Motor Upper(PWM_LU,A_LU,B_LU);
ParinyaT 1:84167ca00307 38 Motor Lower(PWM_LL,A_LL,B_LL);
ParinyaT 0:451c27e4d55e 39 //-- PID --
ParinyaT 1:84167ca00307 40 int Upper_SetPoint;
ParinyaT 1:84167ca00307 41 int Lower_SetPoint;
ParinyaT 1:84167ca00307 42 PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
ParinyaT 1:84167ca00307 43 PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
ParinyaT 0:451c27e4d55e 44 //*****************************************************/
ParinyaT 1:84167ca00307 45 void Read_Encoder(PinName Encoder)
ParinyaT 0:451c27e4d55e 46 {
soulx 7:bf239d051e8c 47 ENC.format(8,0);
soulx 7:bf239d051e8c 48 ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
b0ssiz 6:98871feebea0 49
soulx 7:bf239d051e8c 50 if(Encoder == EncoderA) {
soulx 7:bf239d051e8c 51 EncA = 0;
soulx 7:bf239d051e8c 52 } else {
soulx 7:bf239d051e8c 53 EncB = 0;
soulx 7:bf239d051e8c 54 }
soulx 7:bf239d051e8c 55 ENC.write(0x41);
soulx 7:bf239d051e8c 56 ENC.write(0x09);
soulx 7:bf239d051e8c 57 data = ENC.write(0x00);
soulx 7:bf239d051e8c 58 if(Encoder == EncoderA) {
soulx 7:bf239d051e8c 59 EncA = 1;
soulx 7:bf239d051e8c 60 } else {
soulx 7:bf239d051e8c 61 EncB = 1;
soulx 7:bf239d051e8c 62 }
b0ssiz 6:98871feebea0 63
ParinyaT 0:451c27e4d55e 64 }
ParinyaT 0:451c27e4d55e 65 //*****************************************************/
ParinyaT 1:84167ca00307 66 int Get_EnValue(int Val)
ParinyaT 0:451c27e4d55e 67 {
ParinyaT 0:451c27e4d55e 68 int i = 0;
ParinyaT 0:451c27e4d55e 69 static unsigned char codes[] = {
b0ssiz 6:98871feebea0 70 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
b0ssiz 6:98871feebea0 71 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
b0ssiz 6:98871feebea0 72 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
b0ssiz 6:98871feebea0 73 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
b0ssiz 6:98871feebea0 74 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
b0ssiz 6:98871feebea0 75 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
b0ssiz 6:98871feebea0 76 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
b0ssiz 6:98871feebea0 77 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
b0ssiz 6:98871feebea0 78 };
b0ssiz 6:98871feebea0 79
b0ssiz 6:98871feebea0 80 while(Val != codes[i]) {
ParinyaT 1:84167ca00307 81 i++;
ParinyaT 0:451c27e4d55e 82 }
b0ssiz 6:98871feebea0 83
ParinyaT 1:84167ca00307 84 return i;
b0ssiz 6:98871feebea0 85
ParinyaT 0:451c27e4d55e 86 }
ParinyaT 0:451c27e4d55e 87 //*****************************************************/
ParinyaT 1:84167ca00307 88 void SET_UpperPID()
ParinyaT 1:84167ca00307 89 {
ParinyaT 1:84167ca00307 90 Upper.period(0.001);
ParinyaT 1:84167ca00307 91 Up_PID.setMode(0);
ParinyaT 1:84167ca00307 92 Up_PID.setInputLimits(0,127);
ParinyaT 1:84167ca00307 93 Up_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 94 }
ParinyaT 1:84167ca00307 95 //******************************************************/
ParinyaT 1:84167ca00307 96 void SET_LowerPID()
ParinyaT 1:84167ca00307 97 {
ParinyaT 1:84167ca00307 98 Lower.period(0.001);
ParinyaT 1:84167ca00307 99 Low_PID.setMode(0);
b0ssiz 10:3b3d6bc88677 100 Low_PID.setInputLimits(0,127); // set range
ParinyaT 1:84167ca00307 101 Low_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 102 }
b0ssiz 6:98871feebea0 103 //******************************************************/
ParinyaT 1:84167ca00307 104 void Move_Upper()
ParinyaT 1:84167ca00307 105 {
ParinyaT 1:84167ca00307 106 Read_Encoder(EncoderA);
ParinyaT 1:84167ca00307 107 Upper_Position = Get_EnValue(data);
ParinyaT 1:84167ca00307 108
ParinyaT 1:84167ca00307 109 Up_PID.setProcessValue(Upper_Position);
b0ssiz 6:98871feebea0 110
soulx 7:bf239d051e8c 111 if(Upper_Position - Upper_SetPoint > 0 ) {
ParinyaT 1:84167ca00307 112 dir = 1;
b0ssiz 6:98871feebea0 113 }
soulx 7:bf239d051e8c 114 if(Upper_Position - Upper_SetPoint < 0 ) {
ParinyaT 1:84167ca00307 115 dir = -1;
b0ssiz 6:98871feebea0 116 }
soulx 7:bf239d051e8c 117 Upper.speed(Up_PID.compute() * dir);
ParinyaT 1:84167ca00307 118 }
ParinyaT 1:84167ca00307 119 //******************************************************/
ParinyaT 1:84167ca00307 120 void Move_Lower()
ParinyaT 1:84167ca00307 121 {
ParinyaT 1:84167ca00307 122 Read_Encoder(EncoderB);
ParinyaT 1:84167ca00307 123 Lower_Position = Get_EnValue(data);
ParinyaT 1:84167ca00307 124
ParinyaT 1:84167ca00307 125 Low_PID.setProcessValue(Lower_Position);
b0ssiz 6:98871feebea0 126
b0ssiz 6:98871feebea0 127 if(Lower_Position - Lower_SetPoint > 0 ) {
ParinyaT 1:84167ca00307 128 dir = 1;
b0ssiz 6:98871feebea0 129 }
b0ssiz 6:98871feebea0 130 if(Lower_Position - Lower_SetPoint < 0 ) {
ParinyaT 1:84167ca00307 131 dir = -1;
b0ssiz 6:98871feebea0 132 }
soulx 7:bf239d051e8c 133 Lower.speed(Low_PID.compute() * dir);
b0ssiz 6:98871feebea0 134 }
ParinyaT 1:84167ca00307 135 //******************************************************/
ParinyaT 1:84167ca00307 136
b0ssiz 10:3b3d6bc88677 137 void CmdCheck(uint8_t *cmd)
b0ssiz 10:3b3d6bc88677 138 {
b0ssiz 10:3b3d6bc88677 139 switch(cmd[0]) {
b0ssiz 10:3b3d6bc88677 140 case SET_MOTOR_UPPER_ANG : {
b0ssiz 10:3b3d6bc88677 141 uint8_t Upper_setpoint_buffer[2];
b0ssiz 10:3b3d6bc88677 142 uint8_t Lower_setpoint_buffer[2];
b0ssiz 10:3b3d6bc88677 143 Upper_setpoint_buffer[0]=cmd[1];
b0ssiz 10:3b3d6bc88677 144 Upper_setpoint_buffer[1]=cmd[2];
b0ssiz 10:3b3d6bc88677 145 Upper_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Upper_setpoint_buffer);
b0ssiz 10:3b3d6bc88677 146 Lower_setpoint_buffer[0]=cmd[5];
b0ssiz 10:3b3d6bc88677 147 Lower_setpoint_buffer[1]=cmd[6];
b0ssiz 10:3b3d6bc88677 148 Lower_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Lower_setpoint_buffer);
b0ssiz 10:3b3d6bc88677 149 //Set Set_point
b0ssiz 10:3b3d6bc88677 150 Up_PID.setSetPoint(Upper_SetPoint);
b0ssiz 10:3b3d6bc88677 151 Low_PID.setSetPoint(Lower_SetPoint);
b0ssiz 10:3b3d6bc88677 152
b0ssiz 10:3b3d6bc88677 153 //Control Motor
b0ssiz 10:3b3d6bc88677 154 Move_Upper();
b0ssiz 10:3b3d6bc88677 155 Move_Lower();
b0ssiz 10:3b3d6bc88677 156 }
b0ssiz 10:3b3d6bc88677 157
b0ssiz 10:3b3d6bc88677 158 case GET_MOTOR_UPPER_ANG : {
b0ssiz 10:3b3d6bc88677 159 float up_angle,low_angle;
b0ssiz 10:3b3d6bc88677 160 //
b0ssiz 10:3b3d6bc88677 161 com.sendMotorPos(MY_ID,up_angle,low_angle);
b0ssiz 10:3b3d6bc88677 162 }
b0ssiz 10:3b3d6bc88677 163 case SAVE_DATA_TO_EEPROM : {
b0ssiz 10:3b3d6bc88677 164 uint8_t Int_data_buffer[2];
b0ssiz 10:3b3d6bc88677 165 uint8_t Float_data_buffer[2];
b0ssiz 10:3b3d6bc88677 166 uint8_t Address_buffer[2];
b0ssiz 10:3b3d6bc88677 167 float int_buffer;
b0ssiz 10:3b3d6bc88677 168 float float_buffer;
b0ssiz 10:3b3d6bc88677 169 float data;
b0ssiz 10:3b3d6bc88677 170 uint16_t address;
b0ssiz 10:3b3d6bc88677 171 Int_data_buffer[0]=cmd[1];
b0ssiz 10:3b3d6bc88677 172 Int_data_buffer[1]=cmd[2];
b0ssiz 10:3b3d6bc88677 173 Float_data_buffer[0]=cmd[3];
b0ssiz 10:3b3d6bc88677 174 Float_data_buffer[1]=cmd[4];
b0ssiz 10:3b3d6bc88677 175 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(Int_data_buffer);
b0ssiz 10:3b3d6bc88677 176 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(Float_data_buffer)/FLOAT_CONVERTER;
b0ssiz 10:3b3d6bc88677 177
b0ssiz 10:3b3d6bc88677 178 address=Utilities::ConvertUInt8ArrayToInt16(Address_buffer);
b0ssiz 10:3b3d6bc88677 179 data=int_buffer+float_buffer;
b0ssiz 10:3b3d6bc88677 180
b0ssiz 10:3b3d6bc88677 181 //
b0ssiz 10:3b3d6bc88677 182 }
b0ssiz 10:3b3d6bc88677 183 }
b0ssiz 10:3b3d6bc88677 184 }
b0ssiz 10:3b3d6bc88677 185
ParinyaT 1:84167ca00307 186
ParinyaT 0:451c27e4d55e 187 int main()
ParinyaT 0:451c27e4d55e 188 {
b0ssiz 10:3b3d6bc88677 189 uint8_t id;
b0ssiz 10:3b3d6bc88677 190 uint8_t data_array[10];
ParinyaT 1:84167ca00307 191 SET_UpperPID();
ParinyaT 1:84167ca00307 192 SET_LowerPID();
b0ssiz 10:3b3d6bc88677 193 /*
b0ssiz 10:3b3d6bc88677 194 while(1) {
b0ssiz 10:3b3d6bc88677 195 //Set Set_point
b0ssiz 10:3b3d6bc88677 196 Up_PID.setSetPoint(Upper_SetPoint);
b0ssiz 10:3b3d6bc88677 197 Low_PID.setSetPoint(Lower_SetPoint);
ParinyaT 1:84167ca00307 198
b0ssiz 10:3b3d6bc88677 199 //Control Motor
b0ssiz 10:3b3d6bc88677 200 Move_Upper();
b0ssiz 10:3b3d6bc88677 201 Move_Lower();
b0ssiz 10:3b3d6bc88677 202 }
b0ssiz 10:3b3d6bc88677 203 */
ParinyaT 1:84167ca00307 204
b0ssiz 10:3b3d6bc88677 205 com.ReceiveCommand(id,data_array);
ParinyaT 0:451c27e4d55e 206 }
b0ssiz 6:98871feebea0 207
ParinyaT 0:451c27e4d55e 208
ParinyaT 0:451c27e4d55e 209
b0ssiz 6:98871feebea0 210
b0ssiz 6:98871feebea0 211
b0ssiz 6:98871feebea0 212