900 lines does not show the problem. In the last post I saw that DirInput is defined twice with no error. That may give me two instances of the same name. One in the whole program (global) and one in Loop.
#include <Arduino.h>
#include <MeMegaPi.h>
#include "MeEEPROM.h"
//#include <Wire.h>
//#include <SoftwareSerial.h>
#include <NintendoExtensionCtrl.h>
#include <PID_v1.h>
//#include "Plotter.h"
//#define DEBUG_INFO
//#define DEBUG_INFO
//#define DEBUG_INFO1
//#define DEBUG_INFO1
//#define DEBUG_INFO2
//#define DEBUG_INFO2
//#define DEBUG_CSV_Internal
//#define DEBUG_CSV_Internal
//#define DEBUG_CSV_InlOOP
#define DEBUG_CSV_InlOOP
MeGyro gyro_ext(0,0x68); //external gryo sensor
MeStepperOnBoard steppers[4];
MeEncoderOnBoard Encoder_1(SLOT1);
MeEncoderOnBoard Encoder_2(SLOT2);
typedef struct MeModule
{
int16_t device;
int16_t port;
int16_t slot;
int16_t pin;
int16_t index;
float values[3];
} MeModule;
MeModule modules[12];
int16_t len = 52;
int16_t servo_pins[12]={0,0,0,0,0,0,0,0,0,0,0,0};
//Just for MegaPi
int16_t moveSpeed = 180;
int16_t turnSpeed = 180;
int16_t minSpeed = 45;
int16_t factor = 23;
int16_t distance=0;
int16_t randnum = 0;
int16_t LineFollowFlag=0;
#define BLUETOOTH_MODE 0x00
#define DATA_SERIAL 0
#define DATA_SERIAL1 1
#define DATA_SERIAL2 2
#define DATA_SERIAL3 3
uint8_t command_index = 0;
uint8_t megapi_mode = BLUETOOTH_MODE;
uint8_t index = 0;
uint8_t dataLen;
uint8_t modulesLen=0;
uint8_t prevc=0;
uint8_t BluetoothSource = DATA_SERIAL;
uint8_t serialRead;
uint8_t buffer[52];
uint8_t bufferBt1[52];
uint8_t bufferBt2[52];
double CompAngleY, CompAngleX, GyroXangle;
double LastCompAngleY, LastCompAngleX, LastGyroXangle;
double last_turn_setpoint_filter = 0.0;
double last_speed_setpoint_filter = 0.0;
double last_speed_error_filter = 0.0;
double speed_Integral_average = 0.0;
double angle_speed = 0.0;
float dt;
long lasttime_angle = 0;
long lasttime_speed = 0;
long update_sensor = 0;
long blink_time = 0;
long last_Pulse_pos_encoder1 = 0;
long last_Pulse_pos_encoder2 = 0;
boolean isStart = false;
boolean isAvailable = false;
boolean leftflag;
boolean rightflag;
boolean start_flag = false;
boolean move_flag = false;
boolean blink_flag = false;
String mVersion = "0e.01.016";
double Increment = 0;
//////////////////////////////////////////////////////////////////////////////////////
//float RELAX_ANGLE = -1; //Natural balance angle,should be adjustment according to your own car Moves forward
//float RELAX_ANGLE = -2; //Natural balance angle,should be adjustment according to your own car Moves forward faster
//float RELAX_ANGLE = 2; //Natural balance angle,should be adjustment according to your own car Moves forward
//float RELAX_ANGLE = -5; //Natural balance angle,should be adjustment according to your own car Moves forward
//float RELAX_ANGLE = -7; //Natural balance angle,should be adjustment according to your own car Moves forward slowly
//float RELAX_ANGLE = -8; //Natural balance angle,should be adjustment according to your own car Moves forward slowly
//float RELAX_ANGLE = -9; //Natural balance angle,should be adjustment according to your own car
//float RELAX_ANGLE = -6; //Natural balance angle,should be adjustment according to your own car -6 is measured as correct, but performance needs more. Why?
//float RELAX_ANGLE = -10; //Natural balance angle,should be adjustment according to your own car
//float RELAX_ANGLE = 10; //Natural balance angle,should be adjustment according to your own car
//float RELAX_ANGLE = 20; //Natural balance angle,should be adjustment according to your own car
//float RELAX_ANGLE = -20; //Natural balance angle,should be adjustment according to your own car
//float RELAX_ANGLE = 20; //Natural balance angle,should be adjustment according to your own car
//float RELAX_ANGLE = -6; //Natural balance angle,should be adjustment according to your own car
//float RELAX_ANGLE = 0.0; //Natural balance angle,should be adjustment according to your own car
float RELAX_ANGLE = 10.0; //Natural balance angle,should be adjustment according to your own car
///////////////////////////////////////////
//#define PWM_MIN_OFFSET 0
//#define PWM_MIN_OFFSET 25
float p_value = 10;
float i_value = 2;
float d_value = 3;
float relax_angle = 0;
typedef struct{
double P, I, D;
double Setpoint, Output, Integral,differential, last_error;
} PID;
PID PID_angle, PID_speed, PID_turn;
boolean serialOne = false;
boolean serialTwo = false;
boolean serialThree = false;
int LoopCount = 1;
int Turn_Setpoint = 0;
double Turn_filter = 0;
double Speed_filter =0;
Nunchuk nchuk;
int Loop_num = 0;
double Previous_Program_time = millis();
double loop_time;
double Speed;
double speed_now;
double Speed_error;
double Speed_TIntegral;
double Speed_TOutput;
double Angle_error;
double pwm_left = 0;
double pwm_right = 0;
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
//PID2 myPID(&Input, &Output, &Setpoint,0.1,0.1,0.1, DIRECT); // Flat
//PID2 myPID(&Input, &Output, &Setpoint,0.1,0.1,0.1, DIRECT);
PID2 myPID(&Input, &Output, &Setpoint,0.06,0.005,0.0, DIRECT);
double DirInput;
double DirOutput;
double DirSetpoint;
PID2 DirectionPID (&DirInput, &DirOutput, &DirSetpoint, 10000.0, 10000.0, 2.0, DIRECT);
//Plotter p; // create plotter
double x; // global variables
int Loop_Time;
void PID_angle_compute();
void PID_speed_compute();
void balanced_model();
uint8_t readBuffer(int16_t index);
void readEEPROM();
void readSerial();
boolean read_serial();
void reset();
//double getAngleZ();
void DebugPrint(void){
Serial.print ("temp P:");
Serial.print (p_value);
Serial.print (", temp I:");
Serial.print (i_value);
Serial.print (", temp D:");
Serial.print (d_value);
Serial.print (", relax angle:");
Serial.print (relax_angle);
Serial.print (", megapi mode:");
Serial.println(megapi_mode);}
void isr_process_encoder1(void){
if(digitalRead(Encoder_1.getPortB()) == 0){
Encoder_1.pulsePosMinus();}
else{
Encoder_1.pulsePosPlus();}}
void isr_process_encoder2(void){
if(digitalRead(Encoder_2.getPortB()) == 0){
Encoder_2.pulsePosMinus();}
else{
Encoder_2.pulsePosPlus();}}
void WriteBalancedDataToEEPROM(void){
EEPROM.write(BALANCED_CAR_PARTITION_CHECK, EEPROM_IF_HAVEPID_CHECK1);
Serial.print ("EEPROM_IF_HAVEPID_CHECK1 is: "); Serial.println(EEPROM_IF_HAVEPID_CHECK1);
EEPROM.write(BALANCED_CAR_PARTITION_CHECK + 1, EEPROM_IF_HAVEPID_CHECK2);
EEPROM.write(BALANCED_CAR_START_ADDR, EEPROM_CHECK_START);
EEPROM.put(BALANCED_CAR_NATURAL_BALANCE, RELAX_ANGLE);
EEPROM.put(BALANCED_CAR_ANGLE_PID_ADDR, PID_angle.P);
EEPROM.put(BALANCED_CAR_ANGLE_PID_ADDR+4, PID_angle.I);
EEPROM.put(BALANCED_CAR_ANGLE_PID_ADDR+8, PID_angle.D);
EEPROM.put(BALANCED_CAR_SPEED_PID_ADDR, PID_speed.P);
EEPROM.put(BALANCED_CAR_SPEED_PID_ADDR+4, PID_speed.I);
EEPROM.put(BALANCED_CAR_SPEED_PID_ADDR+8, PID_speed.D);
EEPROM.put(BALANCED_CAR_DIR_PID_ADDR, PID_turn.P);
EEPROM.write(BALANCED_CAR_END_ADDR, EEPROM_CHECK_END);
EEPROM.write(MEGAPI_MODE_START_ADDR, EEPROM_CHECK_START);
EEPROM.write(MEGAPI_MODE_CONFIGURE, megapi_mode);
EEPROM.write(MEGAPI_MODE_END_ADDR, EEPROM_CHECK_END);}
SOLVED
I found it. DirInput is indeed defined twice. There are two of them. So I am past one step.
Now, the next step does not work. DirOutput is always zero. This I can not explain.
Does anyone have an idea?
My_Robot_101.cpp (29.1 KB)
