yyz/bk_bishe_pi
1
0
Fork 0
mirror of https://gitee.com/yyz_o/bk_bishe_pi.git synced 2025-09-07 23:21:26 +00:00
Code Issues Packages Projects Releases Wiki Activity

1

Browse Source
This commit is contained in:
氧原子
2025-05-15 20:30:35 +08:00
parent 69f91d3b52
commit 75f7336601
2 changed files with 244 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

97
文件/hdt.txt Normal file
View File

@@ -0,0 +1,97 @@
# EYEBOT Name
EYEBOT IBot-D
# MOTOR Number | TableName
MOTOR 1 Motor_Table
MOTOR 2 Motor_Table
# Servo Number | Low | High |TableName
SERVO 1 0 255 Servo_Table
SERVO 2 0 255 Servo_Table
SERVO 3 0 255 Servo_Table
SERVO 4 0 255 Servo_Table
# ENCODER Number | Clicks per meter
ENCODER 1 6900
ENCODER 2 6900
# PSD NUMER | TableName
PSD 1 PSD_TableA
PSD 2 PSD_TableA
PSD 3 PSD_TableA
PSD 4 PSD_TableA
# WIFI Default_Hotspot(0)/Slave(1)/Custom_Hotspot(2)
WIFI 0
# HOTSPOT Network_Name | Password
# HOTSPOT rob rasp
# SLAVE Network_Name | Password
SLAVE SSID PASS
# RoBIOS On(1)/Off(0) | FontSize
ROBIOS 1 10
# USB Number | DeviceName
USB 1 EyeBot
USB 2 GPS
# DISPLAY LCD(0)/HDMI(1) | Fullscreen Off/On (0/1) | Rotation (0/1) | Autorefresh (0/1)
# Note: Change of rotation requires 2 reboots
LCD 0 0 0 1
# RPI RaspPi_Ver(1/2/3/4)
RPI 4
# DRIVE Wheel distance (90 or 140mm) | Max Motor Speed | Motor1/left dir. (0 = c/w) | Motor2/right dir. (1=clockwise)
DRIVE 110 200 1 2
#DEMOPATH | Path
DEMOPATH /home/pi/eyebot/demo
#SOFTWAREPATH | Path
SOFTWAREPATH /home/pi/usr
# VOMEGA | Kp | Ki | Kd | Klink (Note if 0,0,0,0 will turn off)
# ... PID frequency, Dead-Rockoning min. change for update, Debug-Flag
VOMEGA 400 40 10 50 10 20 0
# -------------------- TABLES (Optional) ------------------
# Motor Linearisation Table 101 values: 0 .. 100
TABLE Motor_Table
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59
60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79
80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
100
END TABLE
# Servo Linearisation Table 256 values: 0 .. 255
TABLE Servo_Table
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59
60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79
80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119
120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139
140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159
160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179
180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199
200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219
220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239
240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255
END TABLE
# PSD Sensor Linearisation Table 128 values in [mm]:
TABLE PSD_TableA
800 800 800 800 800 800 800 800
800 800 800 800 800 800 800 800 800 800 800 770 750 720 700 680 650 610 590 560
540 530 480 470 460 450 440 430 420 410 400 390 380 370 360 350 340 330 330 320
300 290 280 280 280 270 270 260 250 250 250 240 240 230 230 220 220 210 210 210
200 200 190 190 190 190 180 180 170 170 170 160 160 160 160 150 150 150 150 150
140 140 140 140 140 130 130 130 130 130 130 120 120 120 120 110 110 110 110 110
110 110 110 110 110 110 110 110 110 100 100 100 100 100 100 100 90 90 90 90
END TABLE

147
文件/新建 文本文档.txt Normal file
View File

@@ -0,0 +1,147 @@
#include "eyebot.h"
#define SAFE 200
static int v_des1,v_des2;
float Kp=0.25;
float Ki=0.06;
float Klink=0.05;
TIMER t1;
void picontroller()
{
int enc_new1,v_act1,r_mot1,e_func1;
int enc_new2,v_act2,r_mot2,e_func2;
static int enc_old1,enc_old2;
static int r_old1=0,e_old1=0;
static int r_old2=0,e_old2=0;
enc_new1=ENCODERRead(1);
enc_new2=ENCODERRead(2);
v_act1=enc_new1-enc_old1;
v_act2=enc_new2-enc_old2;
e_func1=v_des1-v_act1;
e_func2=v_des2-v_act2;
r_mot1=r_old1+Kp*(e_func1-e_old1)+Ki*(e_func1+e_old1)/2+Klink*(abs(enc_new1)-abs(enc_new2));
r_mot2=r_old2+Kp*(e_func2-e_old2)+Ki*(e_func2+e_old2)/2-Klink*(abs(enc_new1)-abs(enc_new2));
if(v_des1>0&&v_des2>0)
{
if(r_mot1>35)r_mot1=35;
else if(r_mot1<0) r_mot1=0;
if(r_mot2>35)r_mot2=35;
else if(r_mot2<0) r_mot2=0;
}
else if(v_des1>0&&v_des2<0)
{
if(r_mot1>35)r_mot1=35;
else if(r_mot1<0) r_mot1=0;
if(r_mot2>0)r_mot2=0;
else if(r_mot2<-35) r_mot2=-35;
}
else if(v_des1<0&&v_des2>0)
{
if(r_mot1>0)r_mot1=0;
else if(r_mot1<-35) r_mot1=-35;
if(r_mot2>35)r_mot2=35;
else if(r_mot2<0) r_mot2=0;
}
else if(v_des1==0&&v_des2==0)
{
r_mot1=0;
r_mot2=0;
v_act1=0;
v_act2=0;
enc_old1=0;
r_old1=0;
enc_old2=0;
r_old2=0;
e_func1=0;
e_func2=0;
ENCODERReset(1);
ENCODERReset(2);
}
MOTORDrive(1,r_mot1);
MOTORDrive(2,r_mot2);
enc_old1=enc_new1;
r_old1=r_mot1;
e_old1=e_func1;
enc_old2=enc_new2;
r_old2=r_mot2;
e_old2=e_func2;
printf("%d %d %d %d\n",r_mot1,r_mot2,enc_old1,enc_old2);
}
void VWSpeed(int v1,int v2)
{
v_des1=v1;
v_des2=v2;
}
void drive()
{
do
{
if(PSDGetRaw(PSD_LEFT)<SAFE)
VWSpeed(100,90);
else VWSpeed(90,100);
OSWait(80);
}while(PSDGetRaw(PSD_FRONT)>SAFE);
VWSpeed(0,0);
OSWait(1000);
}
void turn()
{
VWSpeed(50,-50);
while(PSDGetRaw(PSD_FRONT)<400) OSWait(80);
VWSpeed(0,0);
OSWait(1000);
}
int main()
{
LCDMenu("STOP","2","3","END");
ENCODERReset(1);
ENCODERReset(2);
PSDGetRaw(PSD_FRONT);
PSDGetRaw(PSD_LEFT);
t1=OSAttachTimer(100,picontroller);
while(KEYRead()!=KEY1)
{ drive();
turn();
}
OSDetachTimer(t1);
AUBeep();
while(KEYRead()!=KEY4)
{
MOTORDrive(1,0);
MOTORDrive(2,0);
}
return(0);
}