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@15:6ebca0a1aaca, 2016-01-22 (annotated)

Committer:
b0ssiz
Date:
Fri Jan 22 16:40:02 2016 +0000
Revision:
15:6ebca0a1aaca
Parent:
14:28e24fcc5a01
Child:
16:c0a1daeb9fa5
Update; - fixed bug function CmdCheck

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 11:3dd92d1d542c 23 Ticker Recieve;
ParinyaT 1:84167ca00307 24 //-- Communication --
ParinyaT 0:451c27e4d55e 25 Serial PC(D1,D0);
b0ssiz 10:3b3d6bc88677 26 Bear_Receiver com(Tx,Rx,115200);
b0ssiz 14:28e24fcc5a01 27 int16_t MY_ID = 0x01;
ParinyaT 11:3dd92d1d542c 28 //-- Memorry --
ParinyaT 11:3dd92d1d542c 29 EEPROM memory(PB_4,PA_8,0);
b0ssiz 14:28e24fcc5a01 30 uint8_t UpMargin[4];
b0ssiz 14:28e24fcc5a01 31 uint8_t LowMargin[4];
ParinyaT 13:49cb002ad8fd 32 uint8_t Height[4];
ParinyaT 13:49cb002ad8fd 33 uint8_t Wheelpos[4];
ParinyaT 13:49cb002ad8fd 34 uint8_t Mag[24];
ParinyaT 13:49cb002ad8fd 35 uint8_t Offset[4];
b0ssiz 14:28e24fcc5a01 36 uint8_t Body_width[4];
ParinyaT 13:49cb002ad8fd 37 uint8_t Angle_Range_Up[4];
ParinyaT 13:49cb002ad8fd 38 uint8_t Angle_Range_Low[4];
b0ssiz 6:98871feebea0 39 //-- encoder --
ParinyaT 13:49cb002ad8fd 40 float up_angle,low_angle;
ParinyaT 1:84167ca00307 41 int Upper_Position;
ParinyaT 1:84167ca00307 42 int Lower_Position;
ParinyaT 0:451c27e4d55e 43 int data;
ParinyaT 1:84167ca00307 44 SPI ENC(Emosi, Emiso, Esck);
ParinyaT 1:84167ca00307 45 DigitalOut EncA(EncoderA);
ParinyaT 1:84167ca00307 46 DigitalOut EncB(EncoderB);
ParinyaT 0:451c27e4d55e 47 //-- Motor --
ParinyaT 1:84167ca00307 48 int dir;
ParinyaT 1:84167ca00307 49 Motor Upper(PWM_LU,A_LU,B_LU);
ParinyaT 1:84167ca00307 50 Motor Lower(PWM_LL,A_LL,B_LL);
ParinyaT 0:451c27e4d55e 51 //-- PID --
ParinyaT 1:84167ca00307 52 int Upper_SetPoint;
ParinyaT 1:84167ca00307 53 int Lower_SetPoint;
ParinyaT 1:84167ca00307 54 PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
ParinyaT 1:84167ca00307 55 PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
ParinyaT 0:451c27e4d55e 56 //*****************************************************/
ParinyaT 1:84167ca00307 57 void Read_Encoder(PinName Encoder)
ParinyaT 0:451c27e4d55e 58 {
soulx 7:bf239d051e8c 59 ENC.format(8,0);
soulx 7:bf239d051e8c 60 ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
b0ssiz 6:98871feebea0 61
soulx 7:bf239d051e8c 62 if(Encoder == EncoderA) {
soulx 7:bf239d051e8c 63 EncA = 0;
soulx 7:bf239d051e8c 64 } else {
soulx 7:bf239d051e8c 65 EncB = 0;
soulx 7:bf239d051e8c 66 }
soulx 7:bf239d051e8c 67 ENC.write(0x41);
soulx 7:bf239d051e8c 68 ENC.write(0x09);
soulx 7:bf239d051e8c 69 data = ENC.write(0x00);
soulx 7:bf239d051e8c 70 if(Encoder == EncoderA) {
soulx 7:bf239d051e8c 71 EncA = 1;
soulx 7:bf239d051e8c 72 } else {
soulx 7:bf239d051e8c 73 EncB = 1;
soulx 7:bf239d051e8c 74 }
b0ssiz 6:98871feebea0 75
ParinyaT 0:451c27e4d55e 76 }
ParinyaT 0:451c27e4d55e 77 //*****************************************************/
ParinyaT 1:84167ca00307 78 int Get_EnValue(int Val)
ParinyaT 0:451c27e4d55e 79 {
ParinyaT 0:451c27e4d55e 80 int i = 0;
ParinyaT 0:451c27e4d55e 81 static unsigned char codes[] = {
b0ssiz 6:98871feebea0 82 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
b0ssiz 6:98871feebea0 83 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
b0ssiz 6:98871feebea0 84 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
b0ssiz 6:98871feebea0 85 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
b0ssiz 6:98871feebea0 86 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
b0ssiz 6:98871feebea0 87 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
b0ssiz 6:98871feebea0 88 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
b0ssiz 6:98871feebea0 89 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
b0ssiz 6:98871feebea0 90 };
b0ssiz 6:98871feebea0 91
b0ssiz 6:98871feebea0 92 while(Val != codes[i]) {
ParinyaT 1:84167ca00307 93 i++;
ParinyaT 0:451c27e4d55e 94 }
b0ssiz 6:98871feebea0 95
ParinyaT 1:84167ca00307 96 return i;
b0ssiz 6:98871feebea0 97
ParinyaT 0:451c27e4d55e 98 }
ParinyaT 0:451c27e4d55e 99 //*****************************************************/
ParinyaT 1:84167ca00307 100 void SET_UpperPID()
ParinyaT 1:84167ca00307 101 {
ParinyaT 1:84167ca00307 102 Upper.period(0.001);
ParinyaT 1:84167ca00307 103 Up_PID.setMode(0);
ParinyaT 1:84167ca00307 104 Up_PID.setInputLimits(0,127);
ParinyaT 1:84167ca00307 105 Up_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 106 }
ParinyaT 1:84167ca00307 107 //******************************************************/
ParinyaT 1:84167ca00307 108 void SET_LowerPID()
ParinyaT 1:84167ca00307 109 {
ParinyaT 1:84167ca00307 110 Lower.period(0.001);
ParinyaT 1:84167ca00307 111 Low_PID.setMode(0);
b0ssiz 10:3b3d6bc88677 112 Low_PID.setInputLimits(0,127); // set range
ParinyaT 1:84167ca00307 113 Low_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 114 }
b0ssiz 6:98871feebea0 115 //******************************************************/
ParinyaT 1:84167ca00307 116 void Move_Upper()
ParinyaT 1:84167ca00307 117 {
ParinyaT 1:84167ca00307 118 Read_Encoder(EncoderA);
ParinyaT 1:84167ca00307 119 Upper_Position = Get_EnValue(data);
ParinyaT 1:84167ca00307 120
ParinyaT 1:84167ca00307 121 Up_PID.setProcessValue(Upper_Position);
b0ssiz 6:98871feebea0 122
soulx 7:bf239d051e8c 123 if(Upper_Position - Upper_SetPoint > 0 ) {
ParinyaT 1:84167ca00307 124 dir = 1;
b0ssiz 6:98871feebea0 125 }
soulx 7:bf239d051e8c 126 if(Upper_Position - Upper_SetPoint < 0 ) {
ParinyaT 1:84167ca00307 127 dir = -1;
b0ssiz 6:98871feebea0 128 }
soulx 7:bf239d051e8c 129 Upper.speed(Up_PID.compute() * dir);
ParinyaT 1:84167ca00307 130 }
ParinyaT 1:84167ca00307 131 //******************************************************/
ParinyaT 1:84167ca00307 132 void Move_Lower()
ParinyaT 1:84167ca00307 133 {
ParinyaT 1:84167ca00307 134 Read_Encoder(EncoderB);
ParinyaT 1:84167ca00307 135 Lower_Position = Get_EnValue(data);
ParinyaT 1:84167ca00307 136
ParinyaT 1:84167ca00307 137 Low_PID.setProcessValue(Lower_Position);
b0ssiz 6:98871feebea0 138
b0ssiz 6:98871feebea0 139 if(Lower_Position - Lower_SetPoint > 0 ) {
ParinyaT 1:84167ca00307 140 dir = 1;
b0ssiz 6:98871feebea0 141 }
b0ssiz 6:98871feebea0 142 if(Lower_Position - Lower_SetPoint < 0 ) {
ParinyaT 1:84167ca00307 143 dir = -1;
b0ssiz 6:98871feebea0 144 }
soulx 7:bf239d051e8c 145 Lower.speed(Low_PID.compute() * dir);
b0ssiz 6:98871feebea0 146 }
ParinyaT 1:84167ca00307 147 //******************************************************/
ParinyaT 1:84167ca00307 148
b0ssiz 15:6ebca0a1aaca 149 void CmdCheck(int16_t id,uint8_t *cmd,uint8_t ins)
b0ssiz 10:3b3d6bc88677 150 {
b0ssiz 14:28e24fcc5a01 151 if(id==MY_ID) {
b0ssiz 14:28e24fcc5a01 152 switch (ins) {
b0ssiz 14:28e24fcc5a01 153 case PING: {
b0ssiz 14:28e24fcc5a01 154
b0ssiz 14:28e24fcc5a01 155 }
b0ssiz 14:28e24fcc5a01 156 case WRITE_DATA: {
b0ssiz 14:28e24fcc5a01 157 switch (cmd[0]) {
b0ssiz 14:28e24fcc5a01 158 case ID: {
b0ssiz 14:28e24fcc5a01 159 MY_ID = cmd[1];
b0ssiz 14:28e24fcc5a01 160 }
b0ssiz 14:28e24fcc5a01 161 case MOTOR_UPPER_ANG: {
b0ssiz 14:28e24fcc5a01 162 uint8_t IntUpAngle[2],FloatUpAngle[2];
b0ssiz 14:28e24fcc5a01 163 uint8_t IntLowAngle[2],FloatLowAngle[2];
b0ssiz 14:28e24fcc5a01 164 float int_buffer,float_buffer;
b0ssiz 14:28e24fcc5a01 165
b0ssiz 14:28e24fcc5a01 166 IntUpAngle[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 167 IntUpAngle[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 168 FloatUpAngle[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 169 FloatUpAngle[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 170 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
b0ssiz 14:28e24fcc5a01 171 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER;
b0ssiz 14:28e24fcc5a01 172 up_angle=int_buffer+float_buffer;
b0ssiz 14:28e24fcc5a01 173
b0ssiz 14:28e24fcc5a01 174 IntLowAngle[0]=cmd[5];
b0ssiz 14:28e24fcc5a01 175 IntLowAngle[1]=cmd[6];
b0ssiz 14:28e24fcc5a01 176 FloatLowAngle[0]=cmd[7];
b0ssiz 14:28e24fcc5a01 177 FloatLowAngle[1]=cmd[8];
b0ssiz 14:28e24fcc5a01 178 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
b0ssiz 14:28e24fcc5a01 179 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER;
b0ssiz 14:28e24fcc5a01 180 low_angle=int_buffer+float_buffer;;
b0ssiz 14:28e24fcc5a01 181 }
b0ssiz 14:28e24fcc5a01 182 case UP_MARGIN: {
b0ssiz 14:28e24fcc5a01 183 int i;
b0ssiz 14:28e24fcc5a01 184 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 185 UpMargin[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 186 }
b0ssiz 14:28e24fcc5a01 187 }
b0ssiz 14:28e24fcc5a01 188 case LOW_MARGIN: {
b0ssiz 14:28e24fcc5a01 189 int i;
b0ssiz 14:28e24fcc5a01 190 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 191 LowMargin[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 192 }
b0ssiz 14:28e24fcc5a01 193 }
b0ssiz 14:28e24fcc5a01 194 case KP_UPPER_MOTOR: {
b0ssiz 14:28e24fcc5a01 195 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 196 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 197 float Int,Float;
b0ssiz 14:28e24fcc5a01 198 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 199 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 200 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 201 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 202 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 203 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 204 U_Kc=Int+Float;
b0ssiz 14:28e24fcc5a01 205 }
b0ssiz 14:28e24fcc5a01 206 case KI_UPPER_MOTOR: {
b0ssiz 14:28e24fcc5a01 207 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 208 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 209 float Int,Float,KI;
b0ssiz 14:28e24fcc5a01 210 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 211 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 212 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 213 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 214 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 215 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 216 KI=Int+Float;
b0ssiz 14:28e24fcc5a01 217 U_Ti=KI/U_Kc;
b0ssiz 14:28e24fcc5a01 218 }
b0ssiz 14:28e24fcc5a01 219 case KD_UPPER_MOTOR: {
b0ssiz 14:28e24fcc5a01 220 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 221 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 222 float Int,Float,KD;
b0ssiz 14:28e24fcc5a01 223 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 224 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 225 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 226 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 227 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 228 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 229 KD=Int+Float;
b0ssiz 14:28e24fcc5a01 230 U_Td=KD/U_Kc;
b0ssiz 14:28e24fcc5a01 231 }
b0ssiz 14:28e24fcc5a01 232 case KP_LOWER_MOTOR: {
b0ssiz 14:28e24fcc5a01 233 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 234 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 235 float Int,Float;
b0ssiz 14:28e24fcc5a01 236 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 237 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 238 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 239 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 240 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 241 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 242 L_Kc=Int+Float;
b0ssiz 14:28e24fcc5a01 243 }
b0ssiz 14:28e24fcc5a01 244 case KI_LOWER_MOTOR: {
b0ssiz 14:28e24fcc5a01 245 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 246 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 247 float Int,Float,KI;
b0ssiz 14:28e24fcc5a01 248 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 249 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 250 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 251 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 252 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 253 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 254 KI=Int+Float;
b0ssiz 14:28e24fcc5a01 255 L_Ti=KI/L_Kc;
b0ssiz 14:28e24fcc5a01 256 }
b0ssiz 14:28e24fcc5a01 257 case KD_LOWER_MOTOR: {
b0ssiz 14:28e24fcc5a01 258 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 259 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 260 float Int,Float,KD;
b0ssiz 14:28e24fcc5a01 261 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 262 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 263 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 264 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 265 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 266 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 267 KD=Int+Float;
b0ssiz 14:28e24fcc5a01 268 L_Td=KD/L_Kc;
b0ssiz 14:28e24fcc5a01 269 }
b0ssiz 14:28e24fcc5a01 270 case HEIGHT: {
b0ssiz 14:28e24fcc5a01 271 int i;
b0ssiz 14:28e24fcc5a01 272 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 273 Height[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 274 }
b0ssiz 14:28e24fcc5a01 275 }
b0ssiz 14:28e24fcc5a01 276 case WHEELPOS: {
b0ssiz 14:28e24fcc5a01 277 int i;
b0ssiz 14:28e24fcc5a01 278 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 279 Wheelpos[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 280 }
b0ssiz 14:28e24fcc5a01 281 }
b0ssiz 14:28e24fcc5a01 282 case MAG_DATA: {
b0ssiz 14:28e24fcc5a01 283 int i;
b0ssiz 14:28e24fcc5a01 284 for(i=0; i<24; i++) {
b0ssiz 14:28e24fcc5a01 285 Mag[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 286 }
b0ssiz 14:28e24fcc5a01 287 }
b0ssiz 14:28e24fcc5a01 288 case OFFSET: {
b0ssiz 14:28e24fcc5a01 289 int i;
b0ssiz 14:28e24fcc5a01 290 for(i=0; i<8; i++) {
b0ssiz 14:28e24fcc5a01 291 Offset[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 292 }
b0ssiz 14:28e24fcc5a01 293 }
b0ssiz 14:28e24fcc5a01 294 case BODY_WIDTH: {
b0ssiz 14:28e24fcc5a01 295 int i;
b0ssiz 14:28e24fcc5a01 296 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 297 Body_width[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 298 }
b0ssiz 14:28e24fcc5a01 299 }
b0ssiz 14:28e24fcc5a01 300 case ANGLE_RANGE_UP: {
b0ssiz 14:28e24fcc5a01 301 int i;
b0ssiz 14:28e24fcc5a01 302 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 303 Angle_Range_Up[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 304 }
b0ssiz 14:28e24fcc5a01 305 }
b0ssiz 14:28e24fcc5a01 306 case ANGLE_RANGE_LOW: {
b0ssiz 14:28e24fcc5a01 307 int i;
b0ssiz 14:28e24fcc5a01 308 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 309 Angle_Range_Low[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 310 }
ParinyaT 13:49cb002ad8fd 311 }
ParinyaT 12:6b3b997dd7c2 312 }
b0ssiz 14:28e24fcc5a01 313 case READ_DATA: {
b0ssiz 14:28e24fcc5a01 314 switch (cmd[0]) {
b0ssiz 14:28e24fcc5a01 315 case MOTOR_UPPER_ANG: {
b0ssiz 14:28e24fcc5a01 316
b0ssiz 14:28e24fcc5a01 317 }
b0ssiz 14:28e24fcc5a01 318 case MOTOR_LOWER_ANG: {
b0ssiz 14:28e24fcc5a01 319
b0ssiz 14:28e24fcc5a01 320 }
b0ssiz 14:28e24fcc5a01 321 case UP_MARGIN: {
b0ssiz 14:28e24fcc5a01 322
b0ssiz 14:28e24fcc5a01 323 }
b0ssiz 14:28e24fcc5a01 324 case LOW_MARGIN: {
b0ssiz 14:28e24fcc5a01 325
b0ssiz 14:28e24fcc5a01 326 }
b0ssiz 14:28e24fcc5a01 327 case PID_UPPER_MOTOR: {
b0ssiz 14:28e24fcc5a01 328
b0ssiz 14:28e24fcc5a01 329 }
b0ssiz 14:28e24fcc5a01 330 case PID_LOWER_MOTOR: {
b0ssiz 14:28e24fcc5a01 331
b0ssiz 14:28e24fcc5a01 332 }
b0ssiz 14:28e24fcc5a01 333 case HEIGHT: {
b0ssiz 14:28e24fcc5a01 334
b0ssiz 14:28e24fcc5a01 335 }
b0ssiz 14:28e24fcc5a01 336 case WHEELPOS: {
b0ssiz 14:28e24fcc5a01 337
b0ssiz 14:28e24fcc5a01 338 }
b0ssiz 14:28e24fcc5a01 339 case MAG_DATA: {
b0ssiz 14:28e24fcc5a01 340
b0ssiz 14:28e24fcc5a01 341 }
b0ssiz 14:28e24fcc5a01 342 case OFFSET: {
b0ssiz 14:28e24fcc5a01 343
b0ssiz 14:28e24fcc5a01 344 }
b0ssiz 14:28e24fcc5a01 345 case BODY_WIDTH: {
b0ssiz 14:28e24fcc5a01 346
b0ssiz 14:28e24fcc5a01 347 }
b0ssiz 14:28e24fcc5a01 348 case ANGLE_RANGE_UP: {
b0ssiz 14:28e24fcc5a01 349
b0ssiz 14:28e24fcc5a01 350 }
b0ssiz 14:28e24fcc5a01 351 case ANGLE_RANGE_LOW: {
b0ssiz 14:28e24fcc5a01 352
b0ssiz 14:28e24fcc5a01 353 }
ParinyaT 13:49cb002ad8fd 354 }
ParinyaT 13:49cb002ad8fd 355 }
ParinyaT 13:49cb002ad8fd 356 }
b0ssiz 10:3b3d6bc88677 357 }
b0ssiz 14:28e24fcc5a01 358 }
b0ssiz 10:3b3d6bc88677 359 }
ParinyaT 11:3dd92d1d542c 360 /******************************************************/
ParinyaT 13:49cb002ad8fd 361 void Start_Up()
ParinyaT 11:3dd92d1d542c 362 {
ParinyaT 11:3dd92d1d542c 363 // wait for reciever
ParinyaT 11:3dd92d1d542c 364 memory.read(0x00,MY_ID);
ParinyaT 11:3dd92d1d542c 365 memory.read(0x04,Upper_SetPoint);
ParinyaT 11:3dd92d1d542c 366 memory.read(0x08,Lower_SetPoint);
ParinyaT 11:3dd92d1d542c 367 memory.read(0x10,U_Kc);
ParinyaT 11:3dd92d1d542c 368 memory.read(0x14,U_Ti);
ParinyaT 11:3dd92d1d542c 369 memory.read(0x18,U_Td);
ParinyaT 11:3dd92d1d542c 370 memory.read(0x1c,L_Kc);
ParinyaT 11:3dd92d1d542c 371 memory.read(0x20,L_Ti);
ParinyaT 11:3dd92d1d542c 372 memory.read(0x24,L_Td);
b0ssiz 14:28e24fcc5a01 373
ParinyaT 11:3dd92d1d542c 374 }
b0ssiz 14:28e24fcc5a01 375 /*******************************************************/
ParinyaT 13:49cb002ad8fd 376 void Rc()
ParinyaT 11:3dd92d1d542c 377 {
ParinyaT 11:3dd92d1d542c 378 uint8_t data_array[10];
b0ssiz 14:28e24fcc5a01 379 uint8_t id;
b0ssiz 14:28e24fcc5a01 380 uint8_t ins;
b0ssiz 14:28e24fcc5a01 381
b0ssiz 14:28e24fcc5a01 382 com.ReceiveCommand(&id,data_array,&ins);
b0ssiz 15:6ebca0a1aaca 383 CmdCheck((int16_t)id,data_array,ins);
ParinyaT 11:3dd92d1d542c 384 }
ParinyaT 11:3dd92d1d542c 385 /*******************************************************/
ParinyaT 0:451c27e4d55e 386 int main()
ParinyaT 0:451c27e4d55e 387 {
ParinyaT 11:3dd92d1d542c 388 Recieve.attach(&Rc,0.2);
ParinyaT 11:3dd92d1d542c 389 Start_Up();
ParinyaT 1:84167ca00307 390 SET_UpperPID();
ParinyaT 1:84167ca00307 391 SET_LowerPID();
ParinyaT 11:3dd92d1d542c 392
b0ssiz 14:28e24fcc5a01 393 while(1) {
b0ssiz 14:28e24fcc5a01 394 //Set Set_point
b0ssiz 14:28e24fcc5a01 395 Up_PID.setSetPoint(Upper_SetPoint);
b0ssiz 14:28e24fcc5a01 396 Low_PID.setSetPoint(Lower_SetPoint);
ParinyaT 1:84167ca00307 397
b0ssiz 14:28e24fcc5a01 398 //Control Motor
b0ssiz 14:28e24fcc5a01 399 Move_Upper();
b0ssiz 14:28e24fcc5a01 400 Move_Lower();
b0ssiz 14:28e24fcc5a01 401 }
ParinyaT 11:3dd92d1d542c 402
ParinyaT 1:84167ca00307 403
ParinyaT 11:3dd92d1d542c 404 //com.ReceiveCommand(id,data_array);
ParinyaT 0:451c27e4d55e 405 }
b0ssiz 6:98871feebea0 406
ParinyaT 0:451c27e4d55e 407
ParinyaT 0:451c27e4d55e 408
b0ssiz 6:98871feebea0 409
b0ssiz 6:98871feebea0 410
b0ssiz 6:98871feebea0 411