.......... continued
** Balancing robot for dummies **
Part seven: Xbee wireless control, KasBot V 3.0
2) Robot side:
On the bot side, the Xbee module is set as per post #107
The datastream is decoded within getMotion()
void getMotion() {
if(!Serial.available()) return;
delay(5);
int controlByte = Serial.read(); // get control byte
if(!Serial.available()) return;
int dataByte_1 = Serial.read(); // get data byte 1
if(!Serial.available()) return;
int dataByte_2 = Serial.read(); // get data byte 2
Serial.flush();
if(controlByte == 'd') { // Nunchuk joystick data
setPoint = -(dataByte_1 - 20)/2; // get forward/backward motion byte
if(abs(setPoint) > 2) {
count = 0;
last_count = 0;
}
turn = (dataByte_2 - 20); // get right/left motion byte
} else bip(bipPin, 10, 1); // wrong header
}
setPoint is used to bias updatePid() and slithly move the bot from vertical, thus initiating the move
void loop() {
// ********************* Get Xbee data *******************************
getMotion();
// ********************* Sensor aquisition & filtering ***************
.....
// *********************** Angle Control and motor drive *************
drive = updatePid(setPoint, actAngle); // PID algorithm
if(abs(actAngle) < 100) {
Drive_Motor(drive);
} else {
Drive_Motor(0); // stop motors if situation is hopeless
setZeroIntegal(); // reset PID Integral accumulaor
}
// ********************* print Debug info ****************************
.....
// *********************** loop timing control ***********************
.....
}
turn becomes a new parameter in Drive_Motor(), to allow right and left motor to run at their own speed
int Drive_Motor(int torque) {
if (torque + turn >= 0) { // drive motors forward
digitalWrite(InA_R, LOW);
digitalWrite(InB_R, HIGH);
torque_R = torque + turn;
if (torque - turn >= 0) { // drive motors forward
digitalWrite(InA_L, LOW);
digitalWrite(InB_L, HIGH);
torque_L = torque - turn;
} else {
digitalWrite(InA_L, HIGH);
digitalWrite(InB_L, LOW);
torque_L = abs(torque - turn);
}
} else { // drive motors backward
digitalWrite(InA_R, HIGH);
digitalWrite(InB_R, LOW);
torque_R = abs(torque + turn);
if (torque - turn >= 0) { // drive motors forward
digitalWrite(InA_L, LOW);
digitalWrite(InB_L, HIGH);
torque_L = torque - turn;
} else {
digitalWrite(InA_L, HIGH);
digitalWrite(InB_L, LOW);
torque_L = abs(torque - turn);
}
}
torque_R = map(torque_R,0,255,K_map,255);
torque_L = map(torque_L,0,255,K_map,255);
torque_R = constrain(torque_R, 0, 255);
torque_L = constrain(torque_L * motor_Offset, 0, 255);
analogWrite(PWM_R,torque_R);
analogWrite(PWM_L,torque_L * motor_Offset); // motors are not built equal...
}
OK guys, this is probably my last significant contribution to this thread :-/
I suddently realize that I have a real life and... some other projects to explore : :
I am sure this thread will keep on, with new bright ideas coming on
What about non linear control ??
What about one wheeled bot as shown by Big Oil ??
"...Two wheeled balancing robots are an area of research that may well provide the future locomotion for everyday robots..."
I really enjoyed all these discussions we had on this common project
Thanks again to everybody for reading and contributing