Stedd
/
BalanceBot
1
0
Fork 0
Code Issues 6 Pull Requests Packages Projects 1 Releases Wiki Activity

Compare commits

base: Stedd:c546c757c9356f759262a55ec5a7a37d1db81269
Branches Tags
Stedd:master
...
compare: Stedd:248cd4dc68cdcc79ccd1b48f8a5274f3a9c476a4
Branches Tags
Stedd:master

10 Commits
c546c757c9 ... 248cd4dc68

Author SHA1 Message Date
Stedd 248cd4dc68 Changed name 2019-12-24 15:58:22 +01:00
Stedd ec12a38ec6 Anti windup 2019-12-24 15:53:26 +01:00
Stedd 7dffe5208d Cleanup and tuning 2019-12-24 01:21:12 +01:00
Stedd 9c363bbe3c Added remote control 2019-12-23 21:00:06 +01:00
Stedd 050c1156f2 Moved interrupt to separate function 2019-12-23 14:13:37 +01:00
Stedd 1f1afebd98 Moved plots to separate function. 
Cleanup
 2019-12-23 14:04:17 +01:00
Stedd 40ae950bb1 Turn controller 
Moved encoder readers and inverse kinematic to execute first in motor
function.
Implemented turn controller.
Cleaned up speed command variables.
 2019-12-23 03:47:50 +01:00
Stedd 54d2701460 Complementary filter bug 2019-12-23 02:52:37 +01:00
Stedd 4129c92d15 Merge remote-tracking branch 'refs/remotes/origin/Linear-algebra-tests' 2019-12-22 22:35:07 +01:00
Stedd 0836937e3f Inverse kinematics of differential drive working 2019-12-22 22:34:02 +01:00
6 changed files with 376 additions and 230 deletions
Show Stats Expand all files Collapse all files

29
Main/IMU.ino
View File

@ -11,7 +11,6 @@ int gx, gy, gz;
float gt;
float acc_pitch;
float pitch_rate;
float dAngle, estAngle;
float pitch = 0;
float pitch_prev = 0;
@ -30,15 +29,9 @@ void readIMU() {
// Gyrocope
// Coordinate system
// gx - Pitch rate
// gy - Roll rate
// gz - Yaw rate
// Gyro is calibrated for +-2000deg/s
// Conversion is happening in GY_85.h line 48-50
gx = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value);
gy = convertInt(IMU.gyro_y( IMU.readGyro() ), gyro_overflow_value);
gz = convertInt(IMU.gyro_z( IMU.readGyro() ), gyro_overflow_value);
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
@ -54,24 +47,10 @@ void readIMU() {
//Complementary filter
dAngle = pitch_rate * dT * pow(10, -6);
pitch = acc_pitch * (1 - alpha) + (dAngle + pitch_prev * alpha);
pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha);
pitch_prev = pitch;
//Serial plotter
// Serial.print ( "Pitch:" );
// Serial.print ( pitch );
// Serial.print (" ");
// Serial.print ( "Accelerometer_Pitch:" );
// Serial.print ( acc_pitch );
// Serial.print (" ");
// Serial.print ( "," );
// Serial.println ( gz );
// Serial.print ( "," );
// Serial.println ( gt );
// Serial.print ( "," );
// Serial.println ( acc_pitch);
}

160
Main/Main.ino
View File

@ -1,10 +1,11 @@
//Import
#include <GY_85.h>
#include <Wire.h>
#include <MatrixMath.h>
//Declare library objects
GY_85 IMU;
GY_85 IMU;
//GPIO PIN MAPPING
@ -24,127 +25,19 @@ const byte IMU_I2C_SDA = 27;
unsigned long tNow = micros();
unsigned long tLast = micros() + 13000;
int dT = 0;
float dT_s = 0.0;
//Motor variables
const int PWM_CYCLE = 12000;
const byte PWM_RESOLUTION = 12;
const int PWM_CYCLE = 12000;
const byte PWM_RES = 12;
//Encoders variables
long int m1Raw, m1RawLast;
long int m2Raw, m2RawLast;
volatile bool M1_A_state;
volatile bool M1_B_state;
volatile bool M2_A_state;
volatile bool M2_B_state;
//Interrupt routines
// CW = INCREASE
// CCW = DECREASE
void IRAM_ATTR m1_A_changed() {
M1_A_state = digitalRead(M1_ENC_A);
M1_B_state = digitalRead(M1_ENC_B);
//Rising
if (M1_A_state == HIGH) {
if (M1_B_state == HIGH) {
m1Raw = m1Raw - 1;
}
else if (M1_B_state == LOW) {
m1Raw = m1Raw + 1;
}
}
//Falling
else if (M1_A_state == LOW) {
if (M1_B_state == HIGH) {
m1Raw = m1Raw + 1;
}
else if (M1_B_state == LOW) {
m1Raw = m1Raw - 1;
}
}
}
void IRAM_ATTR m1_B_changed() {
M1_A_state = digitalRead(M1_ENC_A);
M1_B_state = digitalRead(M1_ENC_B);
//Rising
if (M1_B_state == HIGH) {
if (M1_A_state == HIGH) {
m1Raw = m1Raw + 1;
}
else if (M1_A_state == LOW) {
m1Raw = m1Raw - 1;
}
}
//Falling
else if (M1_B_state == LOW) {
if (M1_A_state == HIGH) {
m1Raw = m1Raw - 1;
}
else if (M1_A_state == LOW) {
m1Raw = m1Raw + 1;
}
}
}
void IRAM_ATTR m2_A_changed() {
M2_A_state = digitalRead(M2_ENC_A);
M2_B_state = digitalRead(M2_ENC_B);
//Rising
if (M2_A_state == HIGH) {
if (M2_B_state == HIGH) {
m2Raw = m2Raw + 1;
}
else if (M2_B_state == LOW) {
m2Raw = m2Raw - 1;
}
}
//Falling
else if (M2_A_state == LOW) {
if (M2_B_state == HIGH) {
m2Raw = m2Raw - 1;
}
else if (M2_B_state == LOW) {
m2Raw = m2Raw + 1;
}
}
}
void IRAM_ATTR m2_B_changed() {
M2_A_state = digitalRead(M2_ENC_A);
M2_B_state = digitalRead(M2_ENC_B);
//Rising
if (M2_B_state == HIGH) {
if (M2_A_state == HIGH) {
m2Raw = m2Raw - 1;
}
else if (M2_A_state == LOW) {
m2Raw = m2Raw + 1;
}
}
//Falling
else if (M2_B_state == LOW) {
if (M2_A_state == HIGH) {
m2Raw = m2Raw + 1;
}
else if (M2_A_state == LOW) {
m2Raw = m2Raw - 1;
}
}
}
volatile bool M1_A_state, M1_B_state;
volatile bool M2_A_state, M2_B_state;
void setup() {
@ -163,14 +56,7 @@ void setup() {
delay(10);
//Initialize encoder interrupts
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);
attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE);
attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE);
attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE);
initInterrupt();
//Initialize encoders
m1Raw = 0;
@ -179,18 +65,21 @@ void setup() {
m2RawLast = 100;
// Initialize PWM channels
// channels 0-15, resolution 1-16 bits, freq limits depend on resolution
// ledcSetup(uint8_t channel, uint32_t freq, uint8_t resolution_bits);
ledcAttachPin(M1_A, 1);
ledcAttachPin(M1_B, 2);
ledcAttachPin(M2_A, 3);
ledcAttachPin(M2_B, 4);
ledcSetup(1, PWM_CYCLE, PWM_RESOLUTION);
ledcSetup(2, PWM_CYCLE, PWM_RESOLUTION);
ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
ledcSetup(1, PWM_CYCLE, PWM_RES);
ledcSetup(2, PWM_CYCLE, PWM_RES);
ledcSetup(3, PWM_CYCLE, PWM_RES);
ledcSetup(4, PWM_CYCLE, PWM_RES);
//Initialize differential drive inverse kinematics
initMotors();
// Initialize Remote control
initRemote();
}
@ -198,20 +87,25 @@ void loop() {
//Update time variables
tNow = micros();
dT = tNow - tLast; //[Cycle time in microseconds]
dT_s = dT * pow(10,-6); //[Cycle time in seconds]
//Get sensor data
readIMU();
//Control motor
//Control motors
motors();
//Save time
// Plot
plot();
//Save time for next cycle
tLast = tNow;
//Delay
delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
delay(5);
}

114
Main/interruptEncoders.ino Normal file
View File

@ -0,0 +1,114 @@
//Interrupt routines
void IRAM_ATTR m1_A_changed() {
M1_A_state = digitalRead(M1_ENC_A);
M1_B_state = digitalRead(M1_ENC_B);
//Rising
if (M1_A_state == HIGH) {
if (M1_B_state == HIGH) {
m1Raw = m1Raw - 1;
}
else if (M1_B_state == LOW) {
m1Raw = m1Raw + 1;
}
}
//Falling
else if (M1_A_state == LOW) {
if (M1_B_state == HIGH) {
m1Raw = m1Raw + 1;
}
else if (M1_B_state == LOW) {
m1Raw = m1Raw - 1;
}
}
}
void IRAM_ATTR m1_B_changed() {
M1_A_state = digitalRead(M1_ENC_A);
M1_B_state = digitalRead(M1_ENC_B);
//Rising
if (M1_B_state == HIGH) {
if (M1_A_state == HIGH) {
m1Raw = m1Raw + 1;
}
else if (M1_A_state == LOW) {
m1Raw = m1Raw - 1;
}
}
//Falling
else if (M1_B_state == LOW) {
if (M1_A_state == HIGH) {
m1Raw = m1Raw - 1;
}
else if (M1_A_state == LOW) {
m1Raw = m1Raw + 1;
}
}
}
void IRAM_ATTR m2_A_changed() {
M2_A_state = digitalRead(M2_ENC_A);
M2_B_state = digitalRead(M2_ENC_B);
//Rising
if (M2_A_state == HIGH) {
if (M2_B_state == HIGH) {
m2Raw = m2Raw + 1;
}
else if (M2_B_state == LOW) {
m2Raw = m2Raw - 1;
}
}
//Falling
else if (M2_A_state == LOW) {
if (M2_B_state == HIGH) {
m2Raw = m2Raw - 1;
}
else if (M2_B_state == LOW) {
m2Raw = m2Raw + 1;
}
}
}
void IRAM_ATTR m2_B_changed() {
M2_A_state = digitalRead(M2_ENC_A);
M2_B_state = digitalRead(M2_ENC_B);
//Rising
if (M2_B_state == HIGH) {
if (M2_A_state == HIGH) {
m2Raw = m2Raw - 1;
}
else if (M2_A_state == LOW) {
m2Raw = m2Raw + 1;
}
}
//Falling
else if (M2_B_state == LOW) {
if (M2_A_state == HIGH) {
m2Raw = m2Raw + 1;
}
else if (M2_A_state == LOW) {
m2Raw = m2Raw - 1;
}
}
}
void initInterrupt(){
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);
attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE);
attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE);
attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE);
}

36
Main/interruptRemote.ino Normal file
View File

@ -0,0 +1,36 @@
//Variables
const byte NO_CHANNELS = 2;
const byte CHANNEL_PINS[] = {12, 14};
volatile unsigned int interruptTime_ch1, interruptTimeLast_ch1;
volatile unsigned int interruptTime_ch2, interruptTimeLast_ch2;
volatile unsigned int pwm_time_ch1, pwm_time_ch2;
void ch1_interrupt() {
interruptTime_ch1 = micros();
if (interruptTime_ch1 >interruptTimeLast_ch1 && (interruptTime_ch1 - interruptTimeLast_ch1)< 2100){
pwm_time_ch1 = interruptTime_ch1 - interruptTimeLast_ch1;
}
interruptTimeLast_ch1 = interruptTime_ch1;
}
void ch2_interrupt() {
interruptTime_ch2 = micros();
if (interruptTime_ch2 > interruptTimeLast_ch2 && (interruptTime_ch2 - interruptTimeLast_ch2)< 2100 ){
pwm_time_ch2 = interruptTime_ch2 - interruptTimeLast_ch2;
}
interruptTimeLast_ch2 = interruptTime_ch2;
}
void initRemote(){
//Ch1
pinMode(CHANNEL_PINS[0], INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[0]), ch1_interrupt, CHANGE);
//Ch2
pinMode(CHANNEL_PINS[1], INPUT);
attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[1]), ch2_interrupt, CHANGE);
}

192
Main/motorControl.ino
View File

@ -1,123 +1,164 @@
//Constants
const int MOTOR_SATURATION = round(pow(2, PWM_RESOLUTION));
const float WHEEL_DIAMETER = 0.067708;
const int MOTOR_SATURATION = round(pow(2, PWM_RES));
const float BASE_WIDTH = 0.1837;
const float WHEEL_DIAMETER = 0.0677;
const float PULSES_PER_TURN = 1320.0;
const float BALANCE_POINT = -0.05;
const float SPEED_REFERENCE = 0.0;
const float BALANCE_POINT = 0.05;
const float SPEED_REF = 0.00;
const float TURN_SPEED_REF = 0.00;
const float DEADBAND_M1_POS = 90.0;
const float DEADBAND_M1_NEG = 90.0;
const float DEADBAND_M2_POS = 90.0;
const float DEADBAND_M2_NEG = 90.0;
//Tuning
const float K_SC = 15.0;
const float K_OL = 13.0;
const float K_IL = 80.0;
const float I_IL = 5.5;
const float filter_gain = 15.0;
const float K_SC = 18.5; //Speed controller gain
const float K_TC = 90.0; //Turn controller gain
const float K_OL = 13.0; //Outer loop balance controller gain
const float K_IL = 72.0; //Inner loop balance controller gain
const float I_IL = 80.0; //Inner loop balance controller Igain
const float filter_gain = 16.0; //Motor speed LPF gain
//Help variables
float M1_Lin_Vel, M2_Lin_Vel;
int Speed_CMD, M1_Speed_CMD, M2_Speed_CMD;
int M1_Speed_CMD, M2_Speed_CMD;
float rem_speed_ref, rem_turn_speed_ref;
float SC_cont_out;
float TC_cont_out;
float OL_cont_out;
float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup;
float ref_SC, act_SC, error_SC, SC_cont_out;
float ref_OL, act_OL, error_OL, OL_cont_out;
float ref_IL, act_IL, error_IL, iError_IL;
//Matrices
mtx_type motor_ang_vel [2][1];
mtx_type vel_Matrix [2][1];
mtx_type inv_Kin [2][2];
void initMotors() {
// Inverse Kinematic matrix of differential drive robot
inv_Kin[0][0] = WHEEL_DIAMETER / 4;
inv_Kin[1][0] = (WHEEL_DIAMETER / 2) / BASE_WIDTH;
inv_Kin[0][1] = WHEEL_DIAMETER / 4;
inv_Kin[1][1] = -(WHEEL_DIAMETER / 2) / BASE_WIDTH;
}
void motors() {
//Calculate wheel angular velocity
motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
//Calculate robot linear and angular velocity
Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
// Remote control commands
if (pwm_time_ch1 == 0 && pwm_time_ch2 == 0){
rem_turn_speed_ref = 0;
rem_speed_ref = 0;
}
else{
rem_turn_speed_ref = floatMap(pwm_time_ch1, 992.0, 2015.0, -3.75, 3.75);
rem_speed_ref = floatMap(pwm_time_ch2, 982.0, 2005.0, -0.35, 0.35);
}
// Speed Controller
ref_SC = SPEED_REFERENCE;
act_SC = (M1_Lin_Vel + M2_Lin_Vel) / 2;
error_SC = ref_SC - act_SC;
SC_cont_out = (error_SC * K_SC);
SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);
// Balance controller
// Outer loop
ref_OL = BALANCE_POINT - SC_cont_out;
act_OL = pitch;
error_OL = ref_OL - act_OL;
OL_cont_out = (error_OL * K_OL);
OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL);
// Inner loop
ref_IL = OL_cont_out;
act_IL = pitch_rate;
error_IL = ref_IL - act_IL;
iError_IL = iError_IL + (error_IL * dT * pow(10, -6) * I_IL);
Speed_CMD = round((error_IL * K_IL) + iError_IL);
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))));
IL_cont_out = round((error_IL * K_IL) + iError_IL);
M1_Speed_CMD = Speed_CMD;
M2_Speed_CMD = Speed_CMD;
// M1_Speed_CMD = 0;
// M2_Speed_CMD = 0;
//Turn controller
TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC);
//Calculate speed from encoders
M1_Lin_Vel = encoderReader(m1Raw, m1RawLast, M1_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT, filter_gain);
M2_Lin_Vel = encoderReader(m2Raw, m2RawLast, M2_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT, filter_gain);
//Sum speed command for motors
M1_Speed_CMD = IL_cont_out - TC_cont_out;
M2_Speed_CMD = IL_cont_out + TC_cont_out;
//Sum speed command for motors
// M1_Speed_CMD = 0;
// M2_Speed_CMD = 0;
//Motor control
motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG);
motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG);
// Serial plotter
Serial.print("Balance_Point:");
Serial.print(ref_OL);
Serial.print(" ");
Serial.print("Pitch_Angle:");
Serial.print(act_OL);
Serial.print(" ");
Serial.print("Speed_CMD:");
Serial.println(Speed_CMD * (100.0 / 4096.0));
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;
//Update variables for next scan cycle
m1RawLast = m1Raw;
m2RawLast = m2Raw;
}
float encoderReader(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, int 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_ * 0.000001);
return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * 0.000001 * filt_gain_);
float PController(float ref_, float act_, float k_){
return (ref_-act_)*k_;
}
void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) {
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 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 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_);
return ang_vel_filtered_ + ((ang_vel_ - ang_vel_filtered_) * dT_ * filt_gain_);
}
float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) {
//Returns anti windup difference
//Calculate channel
byte ch1 = motorID * 2 - 1;
byte ch2 = motorID * 2;
// Speed command saturation
if (speedCMD_ > saturation) {
speedCMD_ = saturation;
}
else if (speedCMD_ < -saturation) {
speedCMD_ = -saturation;
}
byte ch2 = motorID * 2;
byte ch1 = ch2 - 1;
float windup = 0;
//Deadband
else if (speedCMD_ > 0 && speedCMD_ < dbPos_) {
if (speedCMD_ > 0 && speedCMD_ < dbPos_) {
speedCMD_ = dbPos_;
}
else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) {
speedCMD_ = -dbNeg_;
}
//Zero speed if input = 0
else if (speedCMD_ == 0) {
speedCMD_ = 0;
// Speed command saturation
else if (speedCMD_ > saturation) {
windup = saturation-speedCMD_;
speedCMD_ = saturation;
}
else if (speedCMD_ < -saturation) {
windup = saturation-speedCMD_;
speedCMD_ = -saturation;
}
else {
speedCMD_ = speedCMD_;
}
//Apply speed command to PWM output
if (speedCMD_ > 0) {
ledcWrite(ch1, 0);
@ -127,4 +168,11 @@ void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, flo
ledcWrite(ch1, -1 * speedCMD_);
ledcWrite(ch2, 0);
}
else if (speedCMD_ == 0) {
ledcWrite(ch1, 0);
ledcWrite(ch2, 0);
}
return windup;
}

75
Main/plot.ino Normal file
View File

@ -0,0 +1,75 @@
void plot(){
// Time
// Serial.print("dT:");
// Serial.println(dT);
// Serial.print(" ");
// Serial.print("dT_s:");
// Serial.println(dT_s);
// Serial.print(" ");
// IMU
// Serial.print ( "Pitch:" );
// Serial.print ( pitch );
// Serial.print (" ");
// Serial.print ( "Accelerometer_Pitch:" );
// Serial.print ( acc_pitch );
// Serial.print (" ");
// Serial.print ( "," );
// Serial.println ( gz );
// Serial.print ( "," );
// Serial.println ( gt );
// Serial.print ( " " );
// Serial.println ( acc_pitch);
// Remote control
// Serial.print("ch1:");
// Serial.print(pwm_time_ch1);
// Serial.print(" ");
// Serial.print("ch2:");
// Serial.print(pwm_time_ch2);
// Serial.print("ch1mapped:");
// Serial.print(rem_turn_speed_ref);
// Serial.print(" ");
// Serial.print("ch2mapped:");
// Serial.println(rem_speed_ref);
// Encoders
// Serial.print("m1Raw:");
// Serial.print(m1Raw);
// Serial.print(" ");
// Serial.print("m2Raw:");
// Serial.println(m2Raw);
// Motors
// Serial.print("SpeedControllerOut:");
// Serial.print(SC_cont_out);
// Serial.print(" ");
// Serial.print("BalanceOLControllerOut:");
// Serial.print(OL_cont_out);
// Serial.print(" ");
// Serial.print("BalanceILControllerOut:");
// Serial.print(IL_cont_out);
// Serial.print(" ");
// Serial.print("TurnControllerOut:");
// Serial.println(TC_cont_out);
// Serial.print(" ");
// Serial.print("M1_CMD:");
// Serial.print(M1_Speed_CMD);
// Serial.print(" ");
// Serial.print("M2_CMD:");
// Serial.println(M2_Speed_CMD);
// Serial.print("M1_Ang_Vel:");
// Serial.print(motor_ang_vel[0][0]);
// Serial.print(" ");
// Serial.print("M2_Ang_Vel:");
// Serial.print(motor_ang_vel[0][1]);
// Serial.print(" ");
// Serial.print("botLinVel:");
// Serial.print(vel_Matrix[0][0]);
// Serial.print(" ");
// Serial.print("botAngVel:");
// Serial.println(vel_Matrix[1][0]);
}