Skip to content

Instantly share code, notes, and snippets.

@ttchengab
Created March 6, 2017 05:48
Show Gist options
  • Save ttchengab/4ad6be0d203b45dbf2d36931c0c0dd7c to your computer and use it in GitHub Desktop.
Save ttchengab/4ad6be0d203b45dbf2d36931c0c0dd7c to your computer and use it in GitHub Desktop.
#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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment