Cleanup
This commit is contained in:
parent
23c84d2e41
commit
b5a53fa8c9
22
Main/IMU.ino
22
Main/IMU.ino
|
|
@ -17,25 +17,25 @@ float pitch_prev = 0;
|
|||
|
||||
void readIMU() {
|
||||
//Acceletometer
|
||||
ax = convertInt(IMU.accelerometer_x( IMU.readFromAccelerometer() ), acc_overflow_value);
|
||||
ay = convertInt(IMU.accelerometer_y( IMU.readFromAccelerometer() ), acc_overflow_value);
|
||||
az = convertInt(IMU.accelerometer_z( IMU.readFromAccelerometer() ), acc_overflow_value);
|
||||
ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value);
|
||||
ay = convertInt(IMU.accelerometer_y(IMU.readFromAccelerometer()), acc_overflow_value);
|
||||
az = convertInt(IMU.accelerometer_z(IMU.readFromAccelerometer()), acc_overflow_value);
|
||||
|
||||
|
||||
//Magnetometer
|
||||
cx = IMU.compass_x( IMU.readFromCompass() );
|
||||
cy = IMU.compass_y( IMU.readFromCompass() );
|
||||
cz = IMU.compass_z( IMU.readFromCompass() );
|
||||
cx = IMU.compass_x(IMU.readFromCompass());
|
||||
cy = IMU.compass_y(IMU.readFromCompass());
|
||||
cz = IMU.compass_z(IMU.readFromCompass());
|
||||
|
||||
|
||||
// Gyrocope
|
||||
gx = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value); // gx - Pitch rate
|
||||
gy = convertInt(IMU.gyro_y( IMU.readGyro() ), gyro_overflow_value); // gy - Roll rate
|
||||
gz = convertInt(IMU.gyro_z( IMU.readGyro() ), gyro_overflow_value); // gz - Yaw rate
|
||||
gx = convertInt(IMU.gyro_x(IMU.readGyro()), gyro_overflow_value); // gx - Pitch rate
|
||||
gy = convertInt(IMU.gyro_y(IMU.readGyro()), gyro_overflow_value); // gy - Roll rate
|
||||
gz = convertInt(IMU.gyro_z(IMU.readGyro()), gyro_overflow_value); // gz - Yaw rate
|
||||
|
||||
|
||||
//Temperature sensor
|
||||
gt = IMU.temp ( IMU.readGyro() );
|
||||
gt = IMU.temp(IMU.readGyro());
|
||||
|
||||
|
||||
// Pitch angle from accelerometer
|
||||
|
|
@ -49,8 +49,6 @@ void readIMU() {
|
|||
//Complementary filter
|
||||
pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha);
|
||||
pitch_prev = pitch;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -54,12 +54,10 @@ void setup() {
|
|||
|
||||
//Initialize IMU
|
||||
IMU.init();
|
||||
//Might need some logic here to mke sure the gyro is calibrated correctly, or hardcode the values...
|
||||
IMU.GyroCalibrate();
|
||||
delay(10);
|
||||
|
||||
//Initialize encoder interrupts
|
||||
initInterrupt();
|
||||
initEncoderInterrupt();
|
||||
|
||||
//Initialize encoders
|
||||
m1Raw = 0;
|
||||
|
|
@ -83,14 +81,13 @@ void setup() {
|
|||
|
||||
//Initialize PS3 controller connection
|
||||
Ps3.begin(_ps3Address);
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
//Update time variables
|
||||
tNow = micros();
|
||||
dT = tNow - tLast; //[Cycle time in microseconds]
|
||||
dT_s = dT * pow(10,-6); //[Cycle time in seconds]
|
||||
dT_s = dT * pow(10, -6); //[Cycle time in seconds]
|
||||
|
||||
|
||||
//Get sensor data
|
||||
|
|
@ -102,7 +99,7 @@ void loop() {
|
|||
|
||||
|
||||
// Plot
|
||||
plot();
|
||||
//plot();
|
||||
|
||||
|
||||
//Save time for next cycle
|
||||
|
|
@ -111,4 +108,6 @@ void loop() {
|
|||
|
||||
//Delay
|
||||
delay(5);
|
||||
|
||||
//Test
|
||||
}
|
||||
|
|
|
|||
|
|
@ -7,8 +7,7 @@ void IRAM_ATTR m1_A_changed() {
|
|||
if (M1_A_state == HIGH) {
|
||||
if (M1_B_state == HIGH) {
|
||||
m1Raw = m1Raw - 1;
|
||||
}
|
||||
else if (M1_B_state == LOW) {
|
||||
} else if (M1_B_state == LOW) {
|
||||
m1Raw = m1Raw + 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -17,8 +16,7 @@ void IRAM_ATTR m1_A_changed() {
|
|||
else if (M1_A_state == LOW) {
|
||||
if (M1_B_state == HIGH) {
|
||||
m1Raw = m1Raw + 1;
|
||||
}
|
||||
else if (M1_B_state == LOW) {
|
||||
} else if (M1_B_state == LOW) {
|
||||
m1Raw = m1Raw - 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -33,8 +31,7 @@ void IRAM_ATTR m1_B_changed() {
|
|||
if (M1_B_state == HIGH) {
|
||||
if (M1_A_state == HIGH) {
|
||||
m1Raw = m1Raw + 1;
|
||||
}
|
||||
else if (M1_A_state == LOW) {
|
||||
} else if (M1_A_state == LOW) {
|
||||
m1Raw = m1Raw - 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -43,8 +40,7 @@ void IRAM_ATTR m1_B_changed() {
|
|||
else if (M1_B_state == LOW) {
|
||||
if (M1_A_state == HIGH) {
|
||||
m1Raw = m1Raw - 1;
|
||||
}
|
||||
else if (M1_A_state == LOW) {
|
||||
} else if (M1_A_state == LOW) {
|
||||
m1Raw = m1Raw + 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -58,8 +54,7 @@ void IRAM_ATTR m2_A_changed() {
|
|||
if (M2_A_state == HIGH) {
|
||||
if (M2_B_state == HIGH) {
|
||||
m2Raw = m2Raw + 1;
|
||||
}
|
||||
else if (M2_B_state == LOW) {
|
||||
} else if (M2_B_state == LOW) {
|
||||
m2Raw = m2Raw - 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -68,8 +63,7 @@ void IRAM_ATTR m2_A_changed() {
|
|||
else if (M2_A_state == LOW) {
|
||||
if (M2_B_state == HIGH) {
|
||||
m2Raw = m2Raw - 1;
|
||||
}
|
||||
else if (M2_B_state == LOW) {
|
||||
} else if (M2_B_state == LOW) {
|
||||
m2Raw = m2Raw + 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -84,8 +78,7 @@ void IRAM_ATTR m2_B_changed() {
|
|||
if (M2_B_state == HIGH) {
|
||||
if (M2_A_state == HIGH) {
|
||||
m2Raw = m2Raw - 1;
|
||||
}
|
||||
else if (M2_A_state == LOW) {
|
||||
} else if (M2_A_state == LOW) {
|
||||
m2Raw = m2Raw + 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -94,21 +87,23 @@ void IRAM_ATTR m2_B_changed() {
|
|||
else if (M2_B_state == LOW) {
|
||||
if (M2_A_state == HIGH) {
|
||||
m2Raw = m2Raw + 1;
|
||||
}
|
||||
else if (M2_A_state == LOW) {
|
||||
} else if (M2_A_state == LOW) {
|
||||
m2Raw = m2Raw - 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void initInterrupt(){
|
||||
void initEncoderInterrupt() {
|
||||
pinMode(M1_ENC_A, INPUT_PULLUP);
|
||||
pinMode(M1_ENC_B, INPUT_PULLUP);
|
||||
pinMode(M2_ENC_A, INPUT_PULLUP);
|
||||
pinMode(M2_ENC_B, INPUT_PULLUP);
|
||||
attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE);
|
||||
|
||||
pinMode(M1_ENC_B, INPUT_PULLUP);
|
||||
attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE);
|
||||
|
||||
pinMode(M2_ENC_A, INPUT_PULLUP);
|
||||
attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE);
|
||||
|
||||
pinMode(M2_ENC_B, INPUT_PULLUP);
|
||||
attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -32,9 +32,9 @@ float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup;
|
|||
|
||||
|
||||
//Matrices
|
||||
mtx_type motor_ang_vel [2][1];
|
||||
mtx_type vel_Matrix [2][1];
|
||||
mtx_type inv_Kin [2][2];
|
||||
mtx_type motor_ang_vel[2][1];
|
||||
mtx_type vel_Matrix[2][1];
|
||||
mtx_type inv_Kin[2][2];
|
||||
|
||||
|
||||
void initMotors() {
|
||||
|
|
@ -71,7 +71,7 @@ void motors() {
|
|||
ref_IL = OL_cont_out;
|
||||
act_IL = pitch_rate;
|
||||
error_IL = ref_IL - act_IL;
|
||||
iError_IL = iError_IL + (dT_s*(error_IL * I_IL) + (IL_anti_windup*((1/I_IL)+(1/K_IL))));
|
||||
iError_IL = iError_IL + (dT_s * (error_IL * I_IL) + (IL_anti_windup * ((1 / I_IL) + (1 / K_IL))));
|
||||
IL_cont_out = round((error_IL * K_IL) + iError_IL);
|
||||
|
||||
|
||||
|
|
@ -84,39 +84,37 @@ void motors() {
|
|||
M2_Speed_CMD = IL_cont_out + TC_cont_out;
|
||||
|
||||
//Sum speed command for motors
|
||||
// M1_Speed_CMD = 0;
|
||||
// M2_Speed_CMD = 0;
|
||||
M1_Speed_CMD = 0;
|
||||
M2_Speed_CMD = 0;
|
||||
|
||||
|
||||
//Motor control
|
||||
IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG);
|
||||
IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG);
|
||||
IL_anti_windup = IL_anti_windup/2;
|
||||
IL_anti_windup = IL_anti_windup / 2;
|
||||
|
||||
//Update variables for next scan cycle
|
||||
m1RawLast = m1Raw;
|
||||
m2RawLast = m2Raw;
|
||||
|
||||
|
||||
}
|
||||
|
||||
float PController(float ref_, float act_, float k_){
|
||||
return (ref_-act_)*k_;
|
||||
float PController(float ref_, float act_, float k_) {
|
||||
return (ref_ - act_) * k_;
|
||||
}
|
||||
|
||||
|
||||
float floatMap(int in, float inMin, float inMax, float outMin, float outMax){
|
||||
float floatMap(int in, float inMin, float inMax, float outMin, float outMax) {
|
||||
return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
|
||||
}
|
||||
|
||||
float encoderReaderLinVel(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_ ) {
|
||||
float encoderReaderLinVel(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_) {
|
||||
float dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle]
|
||||
float dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]
|
||||
float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_);
|
||||
return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * filt_gain_);
|
||||
}
|
||||
|
||||
float encoderReaderAngVel(int encRaw, int encRawLast, float ang_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_ ) {
|
||||
float encoderReaderAngVel(int encRaw, int encRawLast, float ang_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_) {
|
||||
float dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle]
|
||||
float dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]
|
||||
float ang_vel_ = (dTurn_ * 2 * PI) / (dT_);
|
||||
|
|
@ -132,18 +130,16 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
|
|||
//Deadband
|
||||
if (speedCMD_ > 0 && speedCMD_ < dbPos_) {
|
||||
speedCMD_ = dbPos_;
|
||||
}
|
||||
else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) {
|
||||
} else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) {
|
||||
speedCMD_ = -dbNeg_;
|
||||
}
|
||||
|
||||
// Speed command saturation
|
||||
else if (speedCMD_ > saturation) {
|
||||
windup = saturation-speedCMD_;
|
||||
windup = saturation - speedCMD_;
|
||||
speedCMD_ = saturation;
|
||||
}
|
||||
else if (speedCMD_ < -saturation) {
|
||||
windup = saturation-speedCMD_;
|
||||
} else if (speedCMD_ < -saturation) {
|
||||
windup = saturation - speedCMD_;
|
||||
speedCMD_ = -saturation;
|
||||
}
|
||||
|
||||
|
|
@ -155,16 +151,13 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
|
|||
if (speedCMD_ > 0) {
|
||||
ledcWrite(ch1, 0);
|
||||
ledcWrite(ch2, speedCMD_);
|
||||
}
|
||||
else if (speedCMD_ < 0) {
|
||||
} else if (speedCMD_ < 0) {
|
||||
ledcWrite(ch1, -1 * speedCMD_);
|
||||
ledcWrite(ch2, 0);
|
||||
}
|
||||
else if (speedCMD_ == 0) {
|
||||
} else if (speedCMD_ == 0) {
|
||||
ledcWrite(ch1, 0);
|
||||
ledcWrite(ch2, 0);
|
||||
}
|
||||
|
||||
return windup;
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
void plot(){
|
||||
// Time
|
||||
void plot() {
|
||||
// Time
|
||||
// Serial.print("dT:");
|
||||
// Serial.println(dT);
|
||||
// Serial.print(" ");
|
||||
|
|
@ -7,7 +7,7 @@ void plot(){
|
|||
// Serial.println(dT_s);
|
||||
// Serial.print(" ");
|
||||
|
||||
// IMU
|
||||
// IMU
|
||||
// Serial.print ( "Pitch:" );
|
||||
// Serial.print ( pitch );
|
||||
// Serial.print (" ");
|
||||
|
|
@ -42,7 +42,7 @@ void plot(){
|
|||
// Serial.print("m2Raw:");
|
||||
// Serial.println(m2Raw);
|
||||
|
||||
// Motors
|
||||
// Motors
|
||||
// Serial.print("SpeedControllerOut:");
|
||||
// Serial.print(SC_cont_out);
|
||||
// Serial.print(" ");
|
||||
|
|
|
|||
Loading…
Reference in New Issue