BE@R lab
/
BEAR_Motion
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of
DogPID
by 
Digital B14
Diff: main.cpp
- Revision:
- 18:face01c94152
- Parent:
- 17:4c96838e579f
- Child:
- 19:3da61e637b2d
--- a/main.cpp Sun Jan 24 07:57:55 2016 +0000
+++ b/main.cpp Sun Jan 24 08:26:35 2016 +0000
@@ -154,12 +154,14 @@
if(id==MY_ID) {
switch (ins) {
case PING: {
-
+ break;
}
case WRITE_DATA: {
switch (cmd[0]) {
case ID: {
- MY_ID = cmd[1];
+ ///
+ MY_ID = (int16_t)cmd[1];
+ break;
}
case MOTOR_UPPER_ANG: {
uint8_t IntUpAngle[2],FloatUpAngle[2];
@@ -180,19 +182,22 @@
FloatLowAngle[1]=cmd[8];
int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER;
- low_angle=int_buffer+float_buffer;;
+ low_angle=int_buffer+float_buffer;
+ break;
}
case UP_MARGIN: {
int i;
for(i=0; i<4; i++) {
UpMargin[0+i]=cmd[1+i];
}
+ break;
}
case LOW_MARGIN: {
int i;
for(i=0; i<4; i++) {
LowMargin[0+i]=cmd[1+i];
}
+ break;
}
case KP_UPPER_MOTOR: {
uint8_t int_buffer[2];
@@ -205,6 +210,7 @@
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
U_Kc=Int+Float;
+ break;
}
case KI_UPPER_MOTOR: {
uint8_t int_buffer[2];
@@ -218,6 +224,7 @@
Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
KI=Int+Float;
U_Ti=KI/U_Kc;
+ break;
}
case KD_UPPER_MOTOR: {
uint8_t int_buffer[2];
@@ -231,6 +238,7 @@
Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
KD=Int+Float;
U_Td=KD/U_Kc;
+ break;
}
case KP_LOWER_MOTOR: {
uint8_t int_buffer[2];
@@ -243,6 +251,7 @@
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
L_Kc=Int+Float;
+ break;
}
case KI_LOWER_MOTOR: {
uint8_t int_buffer[2];
@@ -256,6 +265,7 @@
Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
KI=Int+Float;
L_Ti=KI/L_Kc;
+ break;
}
case KD_LOWER_MOTOR: {
uint8_t int_buffer[2];
@@ -269,94 +279,120 @@
Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
KD=Int+Float;
L_Td=KD/L_Kc;
+ break;
}
case HEIGHT: {
int i;
for(i=0; i<4; i++) {
Height[0+i]=cmd[1+i];
}
+ break;
}
case WHEELPOS: {
int i;
for(i=0; i<4; i++) {
Wheelpos[0+i]=cmd[1+i];
}
+ break;
}
case MAG_DATA: {
int i;
for(i=0; i<24; i++) {
Mag[0+i]=cmd[1+i];
}
+ break;
}
case OFFSET: {
int i;
for(i=0; i<8; i++) {
Offset[0+i]=cmd[1+i];
}
+ break;
}
case BODY_WIDTH: {
int i;
for(i=0; i<4; i++) {
Body_width[0+i]=cmd[1+i];
}
+ break;
}
case ANGLE_RANGE_UP: {
int i;
for(i=0; i<8; i++) {
Angle_Range_Up[0+i]=cmd[1+i];
}
+ break;
}
case ANGLE_RANGE_LOW: {
int i;
for(i=0; i<8; i++) {
Angle_Range_Low[0+i]=cmd[1+i];
}
+ break;
}
// unfinish yet!!!!!!!!!!!!!!!!!
case SAVE_EEPROM_DATA: {
- if (cmd[1]==ID){
-
+ if (cmd[1]==ID) {
+
}
+ break;
}
+
+ break;
}
case READ_DATA: {
switch (cmd[0]) {
case MOTOR_UPPER_ANG: {
- com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
+ uint8_t status;
+ status =com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
+ printf("status = 0x%02x\n\r",status);
+ break;
}
case UP_MARGIN: {
- com.sendUpMargin(MY_ID,UpMargin);
+ com.sendUpMargin(MY_ID,UpMargin);
+ break;
}
case LOW_MARGIN: {
com.sendLowMargin(MY_ID,LowMargin);
+ break;
}
case PID_UPPER_MOTOR: {
com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki,U_Kd);
+ break;
}
case PID_LOWER_MOTOR: {
com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki,L_Kd);
+ break;
}
case HEIGHT: {
com.sendHeight(MY_ID,Height);
+ break;
}
case WHEELPOS: {
com.sendWheelPos(MY_ID,Wheelpos);
+ break;
}
case MAG_DATA: {
com.sendMagData(MY_ID,Mag);
+ break;
}
case OFFSET: {
com.sendOffset(MY_ID,Offset);
+ break;
}
case BODY_WIDTH: {
com.sendBodyWidth(MY_ID,Body_width);
+ break;
}
case ANGLE_RANGE_UP: {
com.sendUpAngleRange(MY_ID,Angle_Range_Up);
+ break;
}
case ANGLE_RANGE_LOW: {
com.sendLowAngleRange(MY_ID,Angle_Range_Low);
+ break;
}
+ break;
}
}
}
@@ -385,17 +421,19 @@
uint8_t data_array[30];
uint8_t id;
uint8_t ins;
+ uint8_t status;
- com.ReceiveCommand(&id,data_array,&ins);
- CmdCheck((int16_t)id,data_array,ins);
+ status = com.ReceiveCommand(&id,data_array,&ins);
+ if(status == ANDANTE_ERRBIT_NONE) {
+ CmdCheck((int16_t)id,data_array,ins);
+ }
}
/*******************************************************/
int main()
{
- while(1)
- {
+ while(1) {
Rc();
- }
+ }
/*Recieve.attach(&Rc,0.000001);
Start_Up();
SET_UpperPID();