testing_straight_balance.cpp

#include <stdio.h>
#include <iostream>
#include <algorithm>
#include <cmath>
#include <cassert>
#include <cstring>
#include <cstdint>
#include <libbase/k60/mcg.h>
#include <libsc/system.h>
#include <libsc/k60/ov7725.h>
#include <libsc/led.h>
#include <libsc/st7735r.h>
#include <libutil/misc.h>
#include <libsc/button.h>
#include <libsc/lcd_typewriter.h>
#include <libsc/futaba_s3010.h>
#include <libsc/servo.h>
#include <stdint.h>
#include <inttypes.h>
#include <libutil/string.h>
#include <libsc/encoder.h>
#include <libsc/ab_encoder.h>
#include <libsc/alternate_motor.h>
#include <libsc/motor.h>
#include <libsc/lcd.h>
#include <libsc/tower_pro_mg995.h>
#include <stdio.h>
#include <libsc/encoder.h>
#include <libsc/dir_encoder.h>
#include <libsc/k60/uart_device.h>
#include <libbase/k60/uart.h>
#include <libsc/k60/jy_mcu_bt_106.h>
#include <libsc/mpu6050.h>
#define PI 3.14159265




namespace libbase
{
    namespace k60
    {

        Mcg::Config Mcg::GetMcgConfig()
        {
            Mcg::Config config;
            config.external_oscillator_khz = 50000;
            config.core_clock_khz = 150000;
            return config;
        }

    }
}

using namespace std;
using namespace libsc;
using namespace libbase::k60;
using namespace libsc::k60;

double blut_interval= 15;
double mpu_interval= 5;
double ctrl_interval = 50;
double Kp= 3, Ki= 0, Kd= 0;

int encoder(int power, int val, int encoderval)
{

	if(val < encoderval)	power=power + 5;
	else if(val > encoderval)	power=power - 5;

	return power;


}
bool bluetoothListener(const Byte *data, const size_t size) {
  if (data[0] == 't') {
    tuning = 1;
    inputStr = "";
  } else if (tuning && data[0] != 't') {
    if (data[0] != '\n') {
      inputStr += (char)data[0];
    } else {
      tuning = 0;
      constVector.clear();
      char * pch;
      pch = strtok(&inputStr[0], " ,");
      while (pch != NULL){
        int constant;
        stringstream(pch) >> constant;
        constVector.push_back(constant);
        pch = strtok (NULL, " ,");
      }
      /*if (constVector[0] > 600)
        constVector[0] = 600;
      leftMotorPtr->SetPower(constVector[0]);
      rightMotorPtr->SetPower(constVector[1]);
      */

    //mpu_time_interval = constVector[0];					//(AngleCalculate)
	kp = constVector[0];
	kd = constVector[1];
	mpu_interval = constVector[2];
	ctrl_interval = constVector[3];
	blut_interval = constVector[4]

    }
  }
  return 1;
}
double med_filter(double x, double y, double z)
{

    int count[3][2];
    count[0][0]= x;
    count[1][0]= y;
    count[2][0]= z;
    count[0][1]= 0;
    count[1][1]= 0;
    count[2][1]= 0;

    for(int i = 0; i < 3; i ++)
    {
        if(count[i][0]== max(max(x,y),z)) count[i][1] == 1;
        else if(count[i][0]==min(min(x,y),z))count[i][1] == 1;
    }
    for(int i = 0; i < 3; i ++)
    {
        if(count[i][1]==0) return count[i][0];
    }

}
int main(void)
{
    	System::Init();
//bluetooth config
        JyMcuBt106::Config bluetooth_config;
        bluetooth_config.id=0;
        bluetooth_config.baud_rate=libbase::k60::Uart::Config::BaudRate::k115200;
        bluetooth_config.rx_isr = &bluetoothListener;
    //	bluetooth_config.tx_buf_size = 200;
        JyMcuBt106 bluetooth(bluetooth_config);
//motor config
   	     AlternateMotor::Config motor1_config,motor2_config;
   		 motor1_config.id=0;
       	 AlternateMotor Lmotor(motor1_config);
  	     //Lmotor.SetPower(300);
  	   	 Lmotor.SetClockwise(false);
   	   	 motor2_config.id=1;
       	 AlternateMotor Rmotor(motor2_config);
      	 //Rmotor.SetPower(300);
       	 Rmotor.SetClockwise(true);
       	 int motor1_speed;
       	 int motor2_speed;
         int dir=1;


//servo config
		 FutabaS3010::Config servo_config;
		 servo_config.id=0;
		 FutabaS3010 servo(servo_config);

//camera config
		Ov7725::Config cam_config;
		cam_config.id=0;
		cam_config.w=80;
		cam_config.h=60;
		Ov7725 cam(cam_config);
		Timer::TimerInt t=0;

//accelerometer and gyroscope config
		Mpu6050::Config balance_config;
		Mpu6050 mpu6050(balance_config);

//lcd and typewriter config
//		St7735r::Config lcd_config;
//		St7735r lcd(lcd_config);
//		LcdTypewriter::Config screen_config;
//		screen_config.lcd=&lcd;
//		LcdTypewriter screen(screen_config);

//encoder config
		DirEncoder::Config encoder_config;
		encoder_config.id=1;
		DirEncoder LEncoder(encoder_config);
		encoder_config.id=0;
		DirEncoder REncoder(encoder_config);
		uint32_t left_count,right_count;


		int encoder_val;
    	const Byte *LB = nullptr;
    	double acceleration[3], omega[3];
    	double accx[3][2], accy[3];
    	for(int = 0 ; i < 3; i ++)
        {
            accx[i][1]= 0;
        }
    	int n;

    	double standarddegx= 37.54, standarddegy;
    	double accdegx, accdegy;
    	double gyrodegx= 0, gyrodegy = 0;
    	double degx, degy;
    	char buffer[50];
    	double angular_vx, angular_vy;
    	double PControl, IControl, DControl;

    	double ControlOutput;
    	int Lpower = 0, Rpower = 0; // for straight balancing put the value as 0;
    		while(true){
    			while(t!=System::Time()){
    				t = System::Time();

    				if(t % mpu_interval == 0){
                        mpu6050.Update(1);
                        for(int j = 0; j < 3; j ++)
                        {
                            if(accx[j][1]== min(min(accx[0][1], accx[1][1]),acc[2][1])){
                                accx[j][0]= mpu6050.GetAccel()[0];
                                break;

                            }

                        }
                        acceleration[0]= med_filter(accx[0][0], accx[1][0], accx[2][0]);
    					for(int i = 0; i < 3; i ++)
    					{


    						//acceleration[i] = mpu6050.GetAccel()[i];    // 16384 per gravitational acceleration
    						omega[i]= mpu6050.GetOmega()[i];
    					}

//print accelerometer degree


    					if(acceleration[0]/16384<=1 && acceleration[0]/16384 >= -1) accdegx = asin(acceleration[0]/16384)*180/PI;
    					else if(acceleration[0]/16384> 1) accdegx = asin(1)*180/PI;
    					else degx = asin(-1)*180/PI;
    					/*
    					n= sprintf(buffer,"Adegx: %.3f", accdegx);
    					lcd.SetRegion(Lcd::Rect(1,1,100,100));    // way of getting accelerometer degree can be changed
    					screen.WriteString(buffer);*/

     					if(acceleration[1]/16384<=1&& acceleration[1]/16384 >= -1) accdegy = asin(acceleration[1]/16384)*180/PI;
     					else if(acceleration[1]/16384> 1) accdegy = asin(1)*180/PI;
     					else degy = asin(-1)*180/PI;
     					/*
    					n= sprintf(buffer,"Adegy: %f", accdegy);
    					lcd.SetRegion(Lcd::Rect(1,11,100,150));
    					screen.WriteString(buffer);*/


//gyro value/131/160*0.01 = integral of angular velocity = angle

    					angular_vx = (omega[0]/20960)*0.1;
    					angular_vy = (omega[1]/20960)*0.1;
    					gyrodegx = gyrodegx + (omega[0]/20960)*0.1;
    					gyrodegy = gyrodegy + (omega[1]/20960)*0.1;

//print gyroscope degree
/*
    					n= sprintf(buffer,"Gdegx: %.3f", gyrodegx);
    					lcd.SetRegion(Lcd::Rect(1,21,100,100));    // way of getting accelerometer degree can be changed
    					screen.WriteString(buffer);

    					n= sprintf(buffer,"Gdegy: %.3f", gyrodegy);
    					lcd.SetRegion(Lcd::Rect(1,31,100,100));    // way of getting accelerometer degree can be changed
    					screen.WriteString(buffer);
*/
//to determine the best ratio for the best degree
    					degx = 0.1*accdegx + 0.9*gyrodegx;
    					degy = 0.1*accdegy + 0.9*gyrodegy;

//encoder control
    					left_count = LEncoder.GetCount();
    					Lpower = encoder(Lpower, left_count, encoder_val));
    					/*n= sprintf(buffer,"LEn: %d", left_count);
    					lcd.SetRegion(Lcd::Rect(1, 41,100,200));
    					screen.WriteString(buffer);*/


    					right_count = REncoder.GetCount();
    					Rpower = encoder(Rpower, right_count, encoder_val));
    					/*n= sprintf(buffer,"REn: %d", right_count);
    					lcd.SetRegion(Lcd::Rect(1, 51,100,200));
    					screen.WriteString(buffer);*/


//Motor Direction and Speed PID
    					PControl = Kp*degx-standarddegx;
    					//IControl = Ki*degx; // is this one needed?
    					DControl = Kd*angular_vx;
    					ControlOutput = PControl + DControl;


    				}

    					if(t%ctrl_interval == 0)
                        {
                        if(ControlOutput> 0)
                        {
                            Lmotor.SetClockwise(true);
                            Rmotor.SetClockwise(true);
                           
                        }
                        else if(ControlOutput < 0)
                        {
                            Lmotor.SetClockwise(false);
                            Rmotor.SetClockwise(false);
                            ControlOutput *=-1;
                        }
                        Lmotor.SetPower(Lpower + ControlOutput);
    					Rmotor.SetPower(Rpower + ControlOutput);

                        }

                        if(t%blut_interval == 0)
                        {
                            bt_pre_time = bt_cur_time;


//

						sprintf(LBuffer, "%.1lf,%.2lf, %.2lf\n",1.0,gyrodegx, accdegx);


						const Byte speedByte = 85;
						bluetooth.SendBuffer(&speedByte, 1);
						bluetooth.SendStr(LBuffer);

                        }

    			}
    		}


    		return 0;


}