The MakeBlock self-balancing software(firmware) works so great that I am only trying to add a Nintendo nunchuck. My addition amounts to 4 or 5 lines of code. But somehow, I do not seem to have it right. Yes, I do need to solder my connections to remove an intermittent connection problem. It is quite annoying.
The physical robot has only two wheels. It is the most stable left to right. So, code for left-right turn is not critical. My quick-and-dirty code worked first time out of the box. I had one polish and was done. The robot has minimum stability forward and backward, so the speed code must have maximum accuracy. The nunchuck has both a vertical and horizontal parameter. Left-right work for the robot. So, my vertical hardware and vertical software is the correct match. Though he direction could be wrong. That is where I lose progress. My best guess is that the speed code is fighting with the balance code.
Because it is a real-time process, I cannot step through the code with debug. The Arduino way is to use a lot of PRINT statements. This too can interfere with performance. And the SERIAL MONITOR flashes by so fast that I cannot read it. If I were using a fully functional ANSI terminal, I could at least create a static form with dynamic values.
When I did the balancing robot on Lego in Java, as the robot was balancing, I would store the values in an array. Then. I found out that Java is a Just-in-time (JIT) process. So, when the compiler kicked in, the robot fell. (The JIT can be turned off, however that is advanced Java. And this is not primarily a Java project.) And then, I would print the array. The array would remember the last of the balance process.
Here I am at a loss as to how to proceed. I know that the nunchuck forward-backward is connected to the robot forward-backward. If I kick the nunchuck forward-backward the robot jumps forward-backward. But it will not actually change location on my desk.
I understand 99% of this code. Perfecting this last 1% is the hard part. As usual.
This is where I struggle. Any incite will be greatly greatly appreciated.
double speed_now = ( Encoder_2.getCurrentSpeed() - Encoder_1.getCurrentSpeed() ) /2;
double Speed; // New
Speed = -0.5*(double)(nchuk.joyY()-127); // New
Speed_filter = (1-0.2)*Speed_filter + 0.2*Speed; // New
if(abs(Speed_filter) > 1){
move_flag = true;}
if( (move_flag==true) && (Speed_filter<5) ){
move_flag = false;
Speed_filter = 0;
PID_speed.Integral = 0;}
double error = speed_now - Speed_filter;
PID_speed.Integral += error;
if(move_flag == true) {
PID_speed.Integral = constrain(PID_speed.Integral , -2000, 2000);
PID_speed.Output = PID_speed.P*error + PID_speed.I*PID_speed.Integral;
PID_speed.Output = constrain(PID_speed.Output , -8.0, 8.0);}
else{
PID_speed.Integral = constrain(PID_speed.Integral , -2000, 2000);
PID_speed.Output = PID_speed.P*speed_now + PID_speed.I*PID_speed.Integral;
PID_speed.Output = constrain(PID_speed.Output , -10.0, 10.0);
speed_Integral_average = 0.8*speed_Integral_average + 0.2*PID_speed.Integral;}
PID_angle.Setpoint = RELAX_ANGLE + PID_speed.Output;
CompAngleX = -gyro_ext.getAngleX();
double error;
error = CompAngleX - PID_angle.Setpoint;
PID_angle.Integral += error;
PID_angle.Integral = constrain(PID_angle.Integral,-100,100);
PID_angle.differential = angle_speed;
PID_angle.Output = PID_angle.P*error + PID_angle.I*PID_angle.Integral + PID_angle.D*PID_angle.differential;
if(PID_angle.Output > 0){
PID_angle.Output = PID_angle.Output + PWM_MIN_OFFSET;}
else{
PID_angle.Output = PID_angle.Output - PWM_MIN_OFFSET;}
Turn_Setpoint = 0.2 * (nchuk.joyX()-133); // New
Turn_filter = (1-0.2)*Turn_filter + 0.2*Turn_Setpoint;
double pwm_left = PID_angle.Output - Turn_filter;
double pwm_right = -PID_angle.Output - Turn_filter;
pwm_left = constrain( pwm_left , -255, 255 );
pwm_right = constrain( pwm_right, -255, 255 );