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@13:49cb002ad8fd, 2016-01-21 (annotated)

Committer:
ParinyaT
Date:
Thu Jan 21 19:01:22 2016 +0000
Revision:
13:49cb002ad8fd
Parent:
12:6b3b997dd7c2
Child:
14:28e24fcc5a01
????????????????????????

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