Skip to content

Instantly share code, notes, and snippets.

@ttchengab
Created January 23, 2017 11:36
Show Gist options
  • Save ttchengab/2ee65fab2a761a0c840936db86f74246 to your computer and use it in GitHub Desktop.
Save ttchengab/2ee65fab2a761a0c840936db86f74246 to your computer and use it in GitHub Desktop.
#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>
/*
servo middle: 720;
right:370
left:1070
*/
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;
int main(void)
{
System::Init();
//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;
St7735r::Config s;
s.is_revert = false;
s.is_bgr = false; //screen init;
s.fps = 100;
St7735r screen(s);
const Byte *LB = nullptr;
bool v[30][80];
int index; //v for value
int areaDiff=0, orgAreaDiff;
int pControl, dControl;
double Kp = 1.5, Kd = 1.5;
cam.Start();
while (true){
while(t!=System::Time()){
t = System::Time();
if(t % 10 == 0){
screen.SetRegion(Lcd::Rect(0,0,80,60));
LB = cam.LockBuffer();
screen.FillBits(St7735r::kBlack,St7735r::kWhite,cam.LockBuffer(),8*cam.GetBufferSize());
orgAreaDiff = areaDiff;
areaDiff = 0;
for(int line=11; line<26; line++){
index=0;
for(int i=0; i<10 ; i++){
for(int j=7; j>=0; j--){
if(((*(LB+line*10+i))>>j)&1==1) v[line][index++] = true;
else v[line][index++] = false;
}
}
for(int i=1;i<79;i++){
if((v[line][i-1]&1)+(v[line][i]&1)+(v[line][i+1]&1) >= 2){
if(i<40) {
areaDiff--;
}
else
{
areaDiff++;
}
}
}
}
pControl = areaDiff * Kp; //to be determined
dControl = (areaDiff-orgAreaDiff) * Kd; //to be determined
if((900+pControl+dControl>1000) ||(900+pControl+dControl<800)) {
servo.SetDegree(900+pControl+dControl);
}
else servo.SetDegree(900);
Lmotor.SetPower(280);
Rmotor.SetPower(280);
//motor speed depends on servo's turning deg, to be determined(maybe pid)
cam.UnlockBuffer();
}
}
}
cam.Stop();
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment