Nouns DAO Noggles Robot

This is a Nouns DAO Noggles Self-Balancing Robot build tutorial, check out https://nouns.wtf/ to learn more about Nouns DAO and peep below if you want to build a robot! If you have a 3d printer I am including Nouns Noggles 3D files for printing as well.

Shoot me an e-mail if you need help, get stuck, or want me to send you the 3d printed parts if you do not have a 3-D Printer: dogtreatrobotfactoryceo@gmail.com

Twitter: https://x.com/TheDogeAcademy

Note that I do not receive any $ for purchases via links below, I just tried to get the cheapest options, if you want to support me just tip to my eth wallet but don't feel obliged: 0xc40BD4b12e8785aC3C570a071cC9A80940617310

Tools

1. 3-D Printer
2. Screwdriver
3. Soldering Kit
4. Extra Wire
5. Glue/Velcro/Magnets/Two Sided Tape

Parts

1. Arduino Board
2. Motor Shield
3. Motors, I use the 330 RPM one
4. Wheels- You can swap the motor and wheels out due to price, you can even print the wheels with TPU
5. Wheel Connector
6. If you want Wi-fi on robot
7. M3 Bolts (short, like 3mm or so, so the motor does not short)
8. Heat shrink for the wires you are soldering
9. Battery
10. Charger for battery

3-D Printed Parts

1. Body
2. Bolts, use metal bolts if you want
3. Noggles

Software

1. Arduino
2. MIT App Inventor

Use this Tinkercad repository : 3D Print Files

Click object and export to export to STL for printing. Please tinker.

Make sure to only download objects one at a time! Click the object, click export as STL one by one.

I may make a detailed video showing how to put it together, for now, just give it a shot lol.

/*The arduino code is a modified version of keyestudio 's arudino tutorial for their self balancing car, I modified the blueotooth controls significantly so if you are trying to tweak the commands and something breaks start there. Added comments to help explain what some functions do. The main line to check out is the one to modify the PID, you need to calibrate it for the robot to self balance. Find the text tune to see which lines to edit. Go to keyestudio' s wiki to learn more.


⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡟⠋⠈⠙⣦⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣠⠤⢤⡀⠀⠀
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡇⠀⠀⠀⠈⢇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠞⠀⠀⢠⡜⣦⠀
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡃⠀⠀⠀⠀⠈⢷⡄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⣠⠀⠀⠀⠀⢻⡘⡇
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠃⠀⠀⠀⠀⠀⠀⠙⢶⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⡠⠚⢀⡼⠃⠀⠀⠀⠀⠸⣇⢳
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣾⠀⣀⠖⠀⠀⠀⠀⠉⠀⠀⠈⠉⠛⠛⡛⢛⠛⢳⡶⠖⠋⠀⢠⡞⠀⠀⠀⠐⠆⠀⠀⣿⢸
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠻⣦⣀⣴⡟⠀⠀⢶⣶⣾⡿⠀⠀⣿⢸
⠀⠀⠀⠀⠀⠀⠀⠀⢀⣤⠞⠁⠀⠀⠀⠀⠀⠀⠀⠀⡠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠻⣏⠀⠀⠀⣶⣿⣿⡇⠀⠀⢏⡞
⠀⠀⠀⠀⠀⠀⢀⡴⠛⠀⠀⠀⠀⠀⠀⠀⠀⢀⢀⡾⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⢦⣤⣾⣿⣿⠋⠀⠀⡀⣾⠁
⠀⠀⠀⠀⠀⣠⠟⠁⠀⠀⠀⣀⠀⠀⠀⠀⢀⡟⠈⢀⣤⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠉⠙⣏⡁⠀⠐⠚⠃⣿⠀
⠀⠀⠀⠀⣴⠋⠀⠀⠀⡴⣿⣿⡟⣷⠀⠀⠊⠀⠴⠛⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠙⠀⠀⠀⠀⢹⡆
⠀⠀⠀⣴⠃⠀⠀⠀⠀⣇⣿⣿⣿⠃⠀⠀⠀⠀⠀⠀⠀⠀⢀⣤⡶⢶⣶⣄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡇
⠀⠀⣸⠃⠀⠀⠀⢠⠀⠊⠛⠉⠁⠀⠀⠀⠀⠀⠀⠀⢲⣾⣿⡏⣾⣿⣿⣿⣿⠖⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢧
⠀⢠⡇⠀⠀⠀⠀⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠈⠛⠿⣽⣿⡿⠏⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜
⢀⡿⠀⠀⠀⠀⢀⣤⣶⣟⣶⣦⣄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡇
⢸⠇⠀⠀⠀⠀⣿⣿⣿⣿⣿⣿⣿⣿⣧⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡇
⣼⠀⢀⡀⠀⠀⢷⣿⣿⣿⣿⣿⣿⡿⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⡇
⡇⠀⠈⠀⠀⠀⣬⠻⣿⣿⣿⡿⠙⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⠁
⢹⡀⠀⠀⠀⠈⣿⣶⣿⣿⣝⡛⢳⠭⠍⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠃⠀
⠸⡇⠀⠀⠀⠀⠙⣿⣿⣿⣿⣿⣿⣷⣦⣀⣀⣀⣤⣤⣴⡶⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⠇⠀⠀
⠀⢿⡄⠀⠀⠀⠀⠀⠙⣇⠉⠉⠙⠛⠻⠟⠛⠛⠉⠙⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠋⠀⠀⠀
⠀⠈⢧⠀⠀⠀⠀⠀⠀⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣠⠞⠁⠀⠀⠀⠀
⠀⠀⠘⢷⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣠⠞⠁⠀⠀⠀⠀⠀⠀
⠀⠀⠀⠀⠱⢆⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⡴⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀
⠀⠀⠀⠀⠀⠀⠛⢦⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⣠⠴⠟⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
⠀⠀⠀⠀⠀⠀⠀⠀⠈⠛⠲⠤⣤⣤⣤⣄⠀⠀⠀⠀⠀⠀⠀⢠⣤⣤⠤⠴⠒⠛⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
*/


#include <Adafruit_PWMServoDriver.h>
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address and I2C interface
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire);
#include <Servo.h>
// Depending on your servo make, the pulse width min and max may vary, you
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 330 // Analog servos run at ~50 Hz updates
Servo left_arm; // create servo object to control a servo
Servo right_arm; // create servo object to control a servo

// our servo # counter
uint8_t servonum = 0;


#include <MsTimer2.h>        //internal timer 2
#include <PinChangeInt.h>    //this library can make all pins of arduino REV4 as external interrupt
#include <MPU6050.h>      //MPU6050 library 
#include <Wire.h>        //IIC communication library 
 
MPU6050 mpu6050;     //Instantiate an MPU6050 object; name mpu6050
int16_t ax, ay, az, gx, gy, gz;     //Define three-axis acceleration, three-axis gyroscope variables
 
//TB6612 pins
const int right_R1=8;    
const int right_R2=12;
const int PWM_R=10;
const int left_L1=6;
const int left_L2=7;
const int PWM_L=9;
 
///////////////////////angle parameters//////////////////////////////
float angle_X; //calculate the inclined angle variable of X-axis by accelerometer  
float angle_Y; //calculate the inclined angle variable of Y-axis by accelerometer
float angle0 = 1; //Actual measured angle (ideally 0 degrees) 
float Gyro_x,Gyro_y,Gyro_z;  //Angular angular velocity for gyroscope calculation 
///////////////////////angle parameters//////////////////////////////
 
///////////////////////Kalman_Filter////////////////////////////
float Q_angle = 0.001;  //Covariance of gyroscope noise
float Q_gyro = 0.003;    //Covariance of gyroscope drift noise
float R_angle = 0.5;    //Covariance of accelerometer
char C_0 = 1;
float dt = 0.005; //The value of dt is the filter sampling time
float K1 = 0.05; //  a function containing the Kalman gain; used to calculate the deviation of the optimal estimate
float K_0,K_1,t_0,t_1;
float angle_err;
float q_bias;    //gyroscope drift
 
float accelz = 0;
float angle;
float angleY_one;
float angle_speed;
 
float Pdot[4] = { 0, 0, 0, 0};
float P[2][2] = {{ 1, 0 }, { 0, 1 }};
float  PCt_0, PCt_1, E;
//////////////////////Kalman_Filter/////////////////////////
 
//////////////////////PID parameters///////////////////////////////
double kp = 30  , ki = 0, kd = 0.62;                   //angle loop parameters, you may need to tune and tweak.
double kp_speed = 5.5, ki_speed = 0.080, kd_speed = 0;   // speed loop parameters new increase speed
double kp_turn = 24, ki_turn = 0, kd_turn = 0.08;       // steering loop parameters
double setp0 = 0; //Angle balance point
int PD_pwm;  //angle output
float pwm1 = 0, pwm2 = 0;
 int position;// new variable for servo
//////////////////Interrupt speed count/////////////////////////////
#define PinA_left 5  //external interrupt
#define PinA_right 4   //external interrupt 
volatile long count_right = 0;//Used to calculate the pulse value calculated by the Hall encoder (the volatile long type is to ensure the value is valid)
volatile long count_left = 0;
int speedcc = 0;
//////////////////////pulse count/////////////////////////
int lz = 0;
int rz = 0;
int rpluse = 0;
int lpluse = 0;
int pulseright,pulseleft;
////////////////////////////////PI variable parameter//////////////////////////
float speeds_filterold=0;
float positions=0;
int flag1;
double PI_pwm;
int cc;
int speedout;
float speeds_filter;
 
//////////////////////////////steering PD///////////////////
int turnmax,turnmin,turnout; 
float Turn_pwm = 0;
int zz=0;
int turncc=0;
 
//Remote control or Bluetooth//
int front = 0;//go front variable
int back = 0;//go back variable
int left = 0;//turn left
int right = 0;//turn right
char val;
 
void setup() 
{
   pwm.begin();// new code from Adafruit PCA9685 to control servos
  /*
     In theory the internal oscillator (clock) is 25MHz but it really isn't
     that precise. You can 'calibrate' this by tweaking this number until
     you get the PWM update frequency you're expecting!
     The int.osc. for the PCA9685 chip is a range between about 23-27MHz and
     is used for calculating things like writeMicroseconds()
     Analog servos run at ~50 Hz updates, It is importaint to use an
     oscilloscope in setting the int.osc frequency for the I2C PCA9685 chip.
     1) Attach the oscilloscope to one of the PWM signal pins and ground on
        the I2C PCA9685 chip you are setting the value for.
     2) Adjust setOscillatorFrequency() until the PWM update frequency is the
        expected value (50Hz for most ESCs)
     Setting the value here is specific to each individual I2C PCA9685 chip and
     affects the calculations for the PWM update frequency.
     Failure to correctly set the int.osc value will cause unexpected PWM results
  */
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
  delay(10);
 

  delay(10);
  //set the motor control pins to OUTPUT
  pinMode(right_R1,OUTPUT);       
  pinMode(right_R2,OUTPUT);
  pinMode(left_L1,OUTPUT);
  pinMode(left_L2,OUTPUT);
  pinMode(PWM_R,OUTPUT);
  pinMode(PWM_L,OUTPUT);
 
  //assign initial state value
  digitalWrite(right_R1,1);
  digitalWrite(right_R2,0);
  digitalWrite(left_L1,0);
  digitalWrite(left_L2,1);
  analogWrite(PWM_R,0);
  analogWrite(PWM_L,0);
 
  pinMode(PinA_left, INPUT);  //Speed encoder input
  pinMode(PinA_right, INPUT);
 
  // join I2C bus
  Wire.begin();                            //join I2C bus sequence 
  Serial.begin(9600);                       //open the serial monitor and set the baud rate to 9600
  delay(1500);
  mpu6050.initialize();                       //initialize MPU6050
  delay(2);
 
  //5ms; use timer2 to set timer interruption (note：using timer2 will affect the PWM output of pin3 pin11)
  MsTimer2::set(5, Interrupt_Service_Routine);    //5ms ; execute the function Interrupt_Service_Routine once
  MsTimer2::start();    //start interrupt
  val = 'S';
  pwm.writeMicroseconds (0,1500);
    pwm.writeMicroseconds (1,1500);
}
 
void loop() 
{
   
   int  i;
  // Used for Bluetooth control
   if(Serial.available()) {
   val = Serial.read();      //assign the value read from serial port to val
   Serial.println(val);
 
  switch(val)             //switch statement
  {
    case 'F': front=250; break;       //if val equals to F，front=250，balance robot goes front. These values must be used in your app for remote control unless you are a shrimp then you can control it with shrimptooth megamindpower
    case 'B': back=-150; break;       //go back
    case 'L': left=1; break;    //turn left
    case 'R': right=1; break;                         // turn right
    case 'S': front=0,back=0,left=0,right=0;break;    //stop
    case 'M': 
   left_treat();
   break;  
    case 'A': right_treat(); break;    //when receiving ‘D’，send value of angle to APP
    case 'N': return_treat();break;    //when receiving ‘D’，send value of angle to APP
    
   }
 
 
 }
   
  //external interrupt; used to calculate the wheel speed
  attachPinChangeInterrupt(PinA_left, Code_left, CHANGE);          //PinA_left  Level change triggers external interrupt;  execute subfunction Code_left
  attachPinChangeInterrupt(PinA_right, Code_right, CHANGE);       //PinA_right Level change triggers external interrupt;  execute subfunction Code_right
}
 
/////////////////////Hall count/////////////////////////
//left speed encoder count
void Code_left() 
{
  count_left ++;
} 
//right speed encoder count
void Code_right() 
{
  count_right ++;
} 
////////////////////pulse count///////////////////////
void countpluse()
{
  lz = count_left;     //assign the value counted by encoder to lz
  rz = count_right;
 
  count_left = 0;     //clear the count quantity
  count_right = 0;
 
  lpluse = lz;
  rpluse = rz;
 
  if ((pwm1 < 0) && (pwm2 < 0))                     //judge the moving direction of balance robot; if go back (PWM the motor voltage is negative), the number of pulses is negative.
  {
    rpluse = -rpluse;
    lpluse = -lpluse;
  }
  else if ((pwm1 > 0) && (pwm2 > 0))                 // if go back (PWM the motor voltage is positive) , the number of pulses is positive.
  {
    rpluse = rpluse;
    lpluse = lpluse;
  }
  else if ((pwm1 < 0) && (pwm2 > 0))                 //judge the moving direction of balance robot; if turn left, right pulse is positive but left pulse is negative.
  {
    rpluse = rpluse;
    lpluse = -lpluse;
  }
  else if ((pwm1 > 0) && (pwm2 < 0))               //judge the moving direction of balance robot; if turn right , right pulse is negative but left pulse is positive.
  {
    rpluse = -rpluse;
    lpluse = lpluse;
  }
 
  //entering interrupt per 5ms，pulse will plus
  pulseright += rpluse;
  pulseleft += lpluse;
}
 
/////////////////////////////////interrupt ////////////////////////////
void Interrupt_Service_Routine()
{
  sei();  //allow overall interrupt
  countpluse();        //pulse count subfunction
  mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     //IIC to get MPU6050 six-axis data ax ay az gx gy gz
  angle_calculate(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);      //get the angle and Kalman filtering
  PD();         //PD control of angle loop 
  anglePWM();
 
  cc++;
  if(cc>=8)     //5*8=40，40ms entering PI count of speed once
  {
    speedpiout();   
    cc=0;  // Clear
  }
  turncc++;         
  if(turncc>4)       //20ms entering PI count of steering once
  {
    turnspin();
    turncc=0;     //Clear
  }
}
///////////////////////////////////////////////////////////
 
/////////////////////////////tilt angle calculation///////////////////////
void angle_calculate(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0,float K1)
{
  float Angle = -atan2(ay , az) * (180/ PI);           //Radial rotation angle calculation formula; the negative sign is direction processing.
  Gyro_x = -gx / 131;              //The X-axis angular velocity calculated by the gyroscope ; the negative sign is the direction processing.
  Kalman_Filter(Angle, Gyro_x);            // Kalman Filter
  // Z-axis angular velocity
  Gyro_z = -gz / 131;                      //speed of Z-axis 
  //accelz = az / 16.4;
 
  float angleAx = -atan2(ax, az) * (180 / PI); //calculate the included angle of X-axis  
  Gyro_y = -gy / 131.00; //angle speed of Y-axis 
  Yiorderfilter(angleAx, Gyro_y); //first-order filtering
}
////////////////////////////////////////////////////////////////
 
///////////////////////////////KalmanFilter/////////////////////
void Kalman_Filter(double angle_m, double gyro_m)
{
  angle += (gyro_m - q_bias) * dt;          //prior estimate
  angle_err = angle_m - angle;
   
  Pdot[0] = Q_angle - P[0][1] - P[1][0];    //The differential of the covariance of the prior estimate error 
  Pdot[1] = - P[1][1];
  Pdot[2] = - P[1][1];
  Pdot[3] = Q_gyro;
   
  P[0][0] += Pdot[0] * dt;    //The integral of the covariance differential of the prior estimate error
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
   
  //Intermediate variables in matrix multiplication
  PCt_0 = C_0 * P[0][0];
  PCt_1 = C_0 * P[1][0];
  //denominator
  E = R_angle + C_0 * PCt_0;
  //gain value
  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;
   
  t_0 = PCt_0;  //Intermediate variables in matrix multiplication 
  t_1 = C_0 * P[0][1];
   
  P[0][0] -= K_0 * t_0;    //the covariance of the prior estimate error 
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;
   
  q_bias += K_1 * angle_err;    //posterior estimate
  angle_speed = gyro_m - q_bias;   //The differential of the output value; get the optimal angular velocity
  angle += K_0 * angle_err; ////posterior estimate; get the optimal angular velocity
}
 
/////////////////////first-order filtering/////////////////
void Yiorderfilter(float angle_m, float gyro_m)
{
  angleY_one = K1 * angle_m + (1 - K1) * (angleY_one + gyro_m * dt);
}
 
//////////////////angle PD////////////////////
void PD()
{
  PD_pwm = kp * (angle + angle0) + kd * angle_speed; //PD angle loop control 
}
 
//////////////////speed PI////////////////////
void speedpiout()
{
  float speeds = (pulseleft + pulseright) * 1.0;      //pulse value of speed  
  pulseright = pulseleft = 0;      //Clear 
  speeds_filterold *= 0.7;         //first-order complementary filtering
  speeds_filter = speeds_filterold + speeds * 0.3;
  speeds_filterold = speeds_filter;
  positions += speeds_filter;
  positions += front;             //Forward control fusion
  positions += back;              //backward control fusion
  positions = constrain(positions, -3550,3550);    //Anti-integral saturation
  PI_pwm = ki_speed * (setp0 - positions) + kp_speed * (setp0 - speeds_filter);      //speed loop controlling PI
}
//////////////////speed PI////////////////////
 
///////////////////////////steering/////////////////////////////////
void turnspin()
{
  int flag = 0;      //
  float turnspeed = 0;
  float rotationratio = 0;
   
  if (left == 1 || right == 1)
  {
    if (flag == 0)                             //judge the speed before turning; increase the car’s flexibility.
    {
      turnspeed = ( pulseright + pulseleft);                      //current speed of car; pulse expression
      flag=1;
    }
    if (turnspeed < 0)                                 //Absolute value of the car's current speed
    {
      turnspeed = -turnspeed;
    }
    if(left==1||right==1)         //if press left key or right key
    {
     turnmax=3;          //maximum value of turning
     turnmin=-3;         // minimum value of turning
    }
    rotationratio = 5 / turnspeed;          //set the value by speed
    if (rotationratio < 0.5)
    {
      rotationratio = 0.5;
    }
      
    if (rotationratio > 5)
    {
      rotationratio = 5;
    }
  }
  else
  {
    rotationratio = 0.5;
    flag = 0;
    turnspeed = 0;
  }
  if (left ==1)//plus by direction parameter
  {
    turnout += rotationratio;
  }
  else if (right == 1 )//plus by direction parameter
  {
    turnout -= rotationratio;
  }
  else turnout = 0;
  if (turnout > turnmax)   turnout = turnmax;//the max value setting of amplitude
  if (turnout < turnmin)   turnout = turnmin;//the min value setting of amplitude 
 
  Turn_pwm = -turnout * kp_turn - Gyro_z * kd_turn;//The rotation PD algorithm controls the fusion speed and Z axis rotation positioning
}
///////////////////////////turning/////////////////////////////////
 
////////////////////////////PWM end value/////////////////////////////
void anglePWM()
{
  pwm2=-PD_pwm - PI_pwm + Turn_pwm;           //assign the end value of PWM to motor
  pwm1=-PD_pwm - PI_pwm - Turn_pwm;
   
  if(pwm1>255)             //limit the PWM value not more than 255
  {
    pwm1=255;
  }
  if(pwm1<-255) 
  {
    pwm1=-255;
  }
  if(pwm2>255)
  {
    pwm2=255;
  }
  if(pwm2<-255)
  {
    pwm2=-255;
  }
 
  if(angle>30 || angle<-30)      //if tilt angle is greater than 25° , motor will stop.
  {
    pwm1=pwm2=0;
  }
 
  // Determine the motor’s steering and speed by the positive and negative of PWM 
  if(pwm2>=0)         
  {
    digitalWrite(left_L1,LOW);
    digitalWrite(left_L2,HIGH);
    analogWrite(PWM_L,pwm2);
  }
  else
  {
    digitalWrite(left_L1,HIGH);
    digitalWrite(left_L2,LOW);
    analogWrite(PWM_L,-pwm2);
  }
 
  if(pwm1>=0)
  {
    digitalWrite(right_R1,HIGH);
    digitalWrite(right_R2,LOW);
    analogWrite(PWM_R,pwm1);
  }
  else
  {
    digitalWrite(right_R1,LOW);
    digitalWrite(right_R2,HIGH);
    analogWrite(PWM_R,-pwm1);
  }
}
void left_treat()
{
 
 pwm.writeMicroseconds(0, 600);

  delay(1000);
  

    pwm.writeMicroseconds(0,2400);
   
  

 delay(1000);
    pwm.writeMicroseconds(0,1500);

  

 delay(1000);

  
}
void right_treat()
{

pwm.writeMicroseconds(1,3000 -  600);
  delay(1000);
  


   pwm.writeMicroseconds(1,3000 - 2400);
  

 delay(1000);
   
   pwm.writeMicroseconds(1,1500);
  

 delay(1000);
 }
void return_treat()
{
 pwm.writeMicroseconds(0, 600);
pwm.writeMicroseconds(1,3000 -  600);
  delay(1000);
  

    pwm.writeMicroseconds(0,2400);
   pwm.writeMicroseconds(1,3000 - 2400);
  

 delay(1000);
    pwm.writeMicroseconds(0,1500);
   pwm.writeMicroseconds(1,1500);
  

 delay(1000);
 
}

You can install the APK below directly onto your android tablet or phone or open this link to use MIT App Inventor to play with/modify/explore the app.

https://gallery.appinventor.mit.edu/?galleryid=dd365543-52a3-4fac-83ed-fc910f1a9eab