Commit e89c7df3 authored by 957dd's avatar 957dd

加入了船的逻辑和更新了注释

parent 5c72890b
...@@ -3,6 +3,9 @@ ...@@ -3,6 +3,9 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include <wiringPi.h>
#include <stdio.h>
#include <math.h>
extern int sda_pin; // GPIO0 extern int sda_pin; // GPIO0
extern int scl_pin; // GPIO1 extern int scl_pin; // GPIO1
......
#ifndef __DELAY_H__ #ifndef __DELAY_H__
#define __DELAY_H__ #define __DELAY_H__
#include <stdio.h>
#include <time.h>
void Delay_Ms(int sec,int msec); void Delay_Ms(int sec,int msec);
......
#ifndef __GPIO_PWM_H__ #ifndef __GPIO_PWM_H__
#define __GPIO_PWM_H__ #define __GPIO_PWM_H__
#include <wiringPi.h>
#include <stdio.h>
#include <stdlib.h>
// 定义 PWM 引脚的 WiringPi 编号 // 定义 PWM 引脚的 WiringPi 编号
void pwm_speed();//pwm控制函数 void pwm_speed();//pwm控制函数
void midde_pwm(); void midde_pwm();//车pwm停止函数
void speed_change(unsigned char *buf);//速度和转向值处理mqtt函数 void speed_change(unsigned char *buf);//速度和转向值处理mqtt函数
void pin_init(); void pin_init();//坦克、车的引脚初始化
void pin_all_default(); void pin_all_default();//拉低车的控制引脚
void pin_value(int pin,int value); void pin_value(int pin,int value);//控制引脚高低
void pin_ship_init();
void ship_speed_change(unsigned char *buf);
void ship_stop_pwm();//船pwm停止函数
void pin_all_ship_default();//拉低船引脚
#endif #endif
\ No newline at end of file
#ifndef __IP_H__ #ifndef __IP_H__
#define __IP_H__ #define __IP_H__
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/socket.h> #include <sys/socket.h>
#include <netinet/in.h> #include <netinet/in.h>
#include <arpa/inet.h> #include <arpa/inet.h>
#include <ifaddrs.h> #include <ifaddrs.h>
extern char ip_address[INET_ADDRSTRLEN];// extern char ip_address[INET_ADDRSTRLEN];//读到的ip地址
int ipaddr(); int ipaddr();//ip地址函数
......
#ifndef __MAIN_H__ #ifndef __MAIN_H__
#define __MAIN_H__ #define __MAIN_H__
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <fcntl.h>
#include <pthread.h>
#include <unistd.h>
#include <termios.h>
#include "gpio_pwm.h"
#include "main.h"
#include "ip.h"
#include "mqtt.h"
#include "delay.h"
#include "opensh.h"
#include "INA226.h"
#define filename "/home/orangepi/car/master/Deviceld.txt" #define filename "/home/orangepi/car/master/Deviceld.txt"
......
#ifndef __MQTT_H__ #ifndef __MQTT_H__
#define __MQTT_H__ #define __MQTT_H__
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <mosquitto.h>
#include <cjson/cJSON.h>
#include<time.h>
#include "ip.h"
#include "opensh.h"
#include "gpio_pwm.h"
#include "INA226.h"
extern char* TOPIC ;//="app2dev/controlcar0004" extern char* TOPIC ;//="app2dev/controlcar0004"
extern char* TOPIC2 ;//="dev2app/controlcar0004" extern char* TOPIC2 ;//="dev2app/controlcar0004"
extern char* TOPIC3;//= "controlcar0004" extern char* TOPIC3;//= "controlcar0004"
...@@ -12,22 +27,22 @@ extern char* TOPIC3;//= "controlcar0004" ...@@ -12,22 +27,22 @@ extern char* TOPIC3;//= "controlcar0004"
#define USERNAME "admin" // 替换为你的用户名 #define USERNAME "admin" // 替换为你的用户名
#define PASSWORD "admin" // 替换为你的密码 #define PASSWORD "admin" // 替换为你的密码
extern struct mosquitto *mosq; extern struct mosquitto *mosq;//创建客服端
extern time_t gStart; extern time_t gStart;//开始时间戳
extern int gPwmCount; // 计数 extern int gPwmCount; // 计数
extern int gmessage_type; extern int gmessage_type;//message消息值
extern int gmessage_2_index;//刷新计时函数
extern int gmessage_2_index; int mqtt_init();//mqtt初始化
void mqtt_create(struct mosquitto *mosq);//创建mqtt客服端
void on_connect(struct mosquitto *mosq, void *obj, int rc);//回调函数
void on_message(struct mosquitto *mosq, void *obj, const struct mosquitto_message *message);//消息回调函数
void mqtt_wirte();//发送mqtt,现为心跳
void mqtt_cycle(struct mosquitto *mosq);//循环
void mqtt_clean(struct mosquitto *mosq);//清理
int mqtt_init();
void mqtt_create(struct mosquitto *mosq);
void on_connect(struct mosquitto *mosq, void *obj, int rc);
void on_message(struct mosquitto *mosq, void *obj, const struct mosquitto_message *message) ;
void mqtt_MAC_wirte();
void mqtt_wirte();
void mqtt_cycle(struct mosquitto *mosq);
void mqtt_clean(struct mosquitto *mosq);
#endif #endif
\ No newline at end of file
#ifndef __opensh_H__ #ifndef __opensh_H__
#define __opensh_H__ #define __opensh_H__
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include "mqtt.h"
//#define SHELLFILE "/home/kickpi/car/master/camopen.sh" int opencamsh();//打开摄像头函数
//#define SHELLFILECLOSE "/home/kickpi/car/master/recam.sh"
extern char gwebcam[254]; void refresh_cam();//刷新摄像头函数
int opencamsh();
void refresh_cam();
#endif #endif
\ No newline at end of file
No preview for this file type
#include <wiringPi.h>
#include <stdio.h>
#include <math.h>
#include "INA226.h" #include "INA226.h"
float currentLSB ; float currentLSB ;
......
No preview for this file type
#include <stdio.h> #include "delay.h"
#include <time.h>
void Delay_Ms(int sec,int msec) void Delay_Ms(int sec,int msec)
{ {
......
No preview for this file type
#include <wiringPi.h>
#include <stdio.h>
#include <stdlib.h>
#include "gpio_pwm.h" #include "gpio_pwm.h"
// 定义 PWM 引脚的 WiringPi 编号 // 定义 PWM 引脚的 WiringPi 编号
#define PWM_PIN_SPEED 21 #define PWM_PIN_SPEED 21
#define PWM_PIN_CHANGE 2 #define PWM_PIN_CHANGE 2
const int gpioWPi[] = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26, 27}; const int gpioWPi[] = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26, 27};//车能使用高低引脚
void pin_init()//初始化引脚 const int gpioWPi_ship[] = {6, 10, 16, 20, 22, 23, 25, 27};//船能使用高低引脚
void pin_init()//初始化引脚,车和坦克共用
{ {
int gpioCount = sizeof(gpioWPi) / sizeof(gpioWPi[0]); int gpioCount = sizeof(gpioWPi) / sizeof(gpioWPi[0]);
// 设置所有GPIO为输出模式 // 设置所有GPIO为输出模式
...@@ -20,7 +19,7 @@ void pin_init()//初始化引脚 ...@@ -20,7 +19,7 @@ void pin_init()//初始化引脚
} }
void pin_all_default()//全部至低电平 void pin_all_default()//全部至低电平,车和坦克共用
{ {
int gpioCount = sizeof(gpioWPi) / sizeof(gpioWPi[0]); int gpioCount = sizeof(gpioWPi) / sizeof(gpioWPi[0]);
for (int i=0;i<gpioCount;i++) for (int i=0;i<gpioCount;i++)
...@@ -29,6 +28,27 @@ void pin_all_default()//全部至低电平 ...@@ -29,6 +28,27 @@ void pin_all_default()//全部至低电平
} }
} }
void pin_ship_init()//初始化引脚,车和坦克共用
{
int gpioCount = sizeof(gpioWPi_ship) / sizeof(gpioWPi_ship[0]);
// 设置所有GPIO为输出模式
for(int i = 0; i < gpioCount; i++) {
pinMode(gpioWPi_ship[i], OUTPUT);
digitalWrite(gpioWPi_ship[i], LOW); // 初始化所有引脚为低电平
}
}
void pin_all_ship_default()//全部至低电平,船用
{
int gpioCount = sizeof(gpioWPi_ship) / sizeof(gpioWPi_ship[0]);
for (int i=0;i<gpioCount;i++)
{
digitalWrite(gpioWPi_ship[i], LOW);
}
}
void pin_value(int pin,int value) void pin_value(int pin,int value)
{ {
...@@ -53,17 +73,26 @@ void pwm_speed() { ...@@ -53,17 +73,26 @@ void pwm_speed() {
pwmSetClock(PWM_PIN_CHANGE,pwm_clock);//=19200*1000/(hz*2000) pwmSetClock(PWM_PIN_CHANGE,pwm_clock);//=19200*1000/(hz*2000)
pwmSetRange(PWM_PIN_CHANGE,1000);//占空比范围 pwmSetRange(PWM_PIN_CHANGE,1000);//占空比范围
pwmWrite(PWM_PIN_SPEED,75);
pwmWrite(PWM_PIN_CHANGE,70);
} }
void midde_pwm()//中间pwm值 void midde_pwm()//车停止pwm值
{ {
pwmWrite(PWM_PIN_SPEED,75); pwmWrite(PWM_PIN_SPEED,75);
pwmWrite(PWM_PIN_CHANGE,70); pwmWrite(PWM_PIN_CHANGE,70);
} }
void ship_stop_pwm()//船停止pwm值
{
pwmWrite(PWM_PIN_SPEED,0);
pwmWrite(PWM_PIN_CHANGE,0);
digitalWrite(5, LOW);
digitalWrite(7, LOW);
digitalWrite(24, LOW);
digitalWrite(26, LOW);
}
/*车的速度转向函数*/
void mode_1_flont(unsigned char gval) void mode_1_flont(unsigned char gval)
{ {
...@@ -304,7 +333,7 @@ void mode_4_right(unsigned char gval) ...@@ -304,7 +333,7 @@ void mode_4_right(unsigned char gval)
} }
} }
void speed_change(unsigned char *buf)//速度和转向引脚数值处理函数 void speed_change(unsigned char *buf)//速度和转向引脚数值处理函数
{ {
unsigned char mode=buf[1]; unsigned char mode=buf[1];
unsigned char val=buf[2]; unsigned char val=buf[2];
...@@ -328,3 +357,243 @@ void speed_change(unsigned char *buf)//速度和转向引脚数值处理函数 ...@@ -328,3 +357,243 @@ void speed_change(unsigned char *buf)//速度和转向引脚数值处理函数
} }
} }
/*船的速度转向*/
void ship_mode_1_flont(unsigned char gval)
{
digitalWrite(5, HIGH);
digitalWrite(7, LOW);
digitalWrite(24, HIGH);
digitalWrite(26, LOW);
if(gval<50)
{
pwmWrite(PWM_PIN_SPEED,0);
pwmWrite(PWM_PIN_CHANGE,0);
}
if(50<=gval&&gval<=55)
{
pwmWrite(PWM_PIN_SPEED,200);
pwmWrite(PWM_PIN_CHANGE,200);
}
if(55<gval&&gval<=60)
{
pwmWrite(PWM_PIN_SPEED,300);
pwmWrite(PWM_PIN_CHANGE,300);
}
if(60<gval&&gval<=65)
{
pwmWrite(PWM_PIN_SPEED,350);
pwmWrite(PWM_PIN_CHANGE,350);
}
if(65<gval&&gval<=70)
{
pwmWrite(PWM_PIN_SPEED,400);
pwmWrite(PWM_PIN_CHANGE,400);
}
if(70<gval&&gval<=75)
{
pwmWrite(PWM_PIN_SPEED,450);
pwmWrite(PWM_PIN_CHANGE,450);
}
if(75<gval&&gval<=90)
{
pwmWrite(PWM_PIN_SPEED,500);
pwmWrite(PWM_PIN_CHANGE,500);
}
if(90<gval&&gval<=100)
{
pwmWrite(PWM_PIN_SPEED,550);
pwmWrite(PWM_PIN_CHANGE,550);
}
if(100<gval&&gval<=110)
{
pwmWrite(PWM_PIN_SPEED,600);
pwmWrite(PWM_PIN_CHANGE,600);
}
if(110<gval&&gval<=120)
{
pwmWrite(PWM_PIN_SPEED,650);
pwmWrite(PWM_PIN_CHANGE,650);
}
if(120<gval&&gval<=130)
{
pwmWrite(PWM_PIN_SPEED,700);
pwmWrite(PWM_PIN_CHANGE,700);
}
if(130<gval&&gval<=140)
{
pwmWrite(PWM_PIN_SPEED,750);
pwmWrite(PWM_PIN_CHANGE,750);
}
if(140<gval&&gval<=150)
{
pwmWrite(PWM_PIN_SPEED,800);
pwmWrite(PWM_PIN_CHANGE,800);
}
if(150<gval&&gval<=160)
{
pwmWrite(PWM_PIN_SPEED,850);
pwmWrite(PWM_PIN_CHANGE,850);
}
if(160<gval&&gval<=170)
{
pwmWrite(PWM_PIN_SPEED,900);
pwmWrite(PWM_PIN_CHANGE,900);
}
if(170<gval&&gval<=180)
{
pwmWrite(PWM_PIN_SPEED,950);
pwmWrite(PWM_PIN_CHANGE,950);
}
if(180<gval&&gval<=190)
{
pwmWrite(PWM_PIN_SPEED,1000);
pwmWrite(PWM_PIN_CHANGE,1000);
}
if(190<gval&&gval<=200)
{
pwmWrite(PWM_PIN_SPEED,1000);
pwmWrite(PWM_PIN_CHANGE,1000);
}
}
void ship_mode_2_back(unsigned char gval)
{
digitalWrite(5, LOW);
digitalWrite(7, HIGH);
digitalWrite(24, LOW);
digitalWrite(26, HIGH);
if(gval<50)
{
pwmWrite(PWM_PIN_SPEED,0);
pwmWrite(PWM_PIN_CHANGE,0);
}
if(50<=gval&&gval<=55)
{
pwmWrite(PWM_PIN_SPEED,200);
pwmWrite(PWM_PIN_CHANGE,200);
}
if(55<gval&&gval<=60)
{
pwmWrite(PWM_PIN_SPEED,300);
pwmWrite(PWM_PIN_CHANGE,300);
}
if(60<gval&&gval<=65)
{
pwmWrite(PWM_PIN_SPEED,350);
pwmWrite(PWM_PIN_CHANGE,350);
}
if(65<gval&&gval<=70)
{
pwmWrite(PWM_PIN_SPEED,400);
pwmWrite(PWM_PIN_CHANGE,400);
}
if(70<gval&&gval<=75)
{
pwmWrite(PWM_PIN_SPEED,450);
pwmWrite(PWM_PIN_CHANGE,450);
}
if(75<gval&&gval<=90)
{
pwmWrite(PWM_PIN_SPEED,500);
pwmWrite(PWM_PIN_CHANGE,500);
}
if(90<gval&&gval<=100)
{
pwmWrite(PWM_PIN_SPEED,550);
pwmWrite(PWM_PIN_CHANGE,550);
}
if(100<gval&&gval<=110)
{
pwmWrite(PWM_PIN_SPEED,600);
pwmWrite(PWM_PIN_CHANGE,600);
}
if(110<gval&&gval<=120)
{
pwmWrite(PWM_PIN_SPEED,650);
pwmWrite(PWM_PIN_CHANGE,650);
}
if(120<gval&&gval<=130)
{
pwmWrite(PWM_PIN_SPEED,700);
pwmWrite(PWM_PIN_CHANGE,700);
}
if(130<gval&&gval<=140)
{
pwmWrite(PWM_PIN_SPEED,750);
pwmWrite(PWM_PIN_CHANGE,750);
}
if(140<gval&&gval<=150)
{
pwmWrite(PWM_PIN_SPEED,800);
pwmWrite(PWM_PIN_CHANGE,800);
}
if(150<gval&&gval<=160)
{
pwmWrite(PWM_PIN_SPEED,850);
pwmWrite(PWM_PIN_CHANGE,850);
}
if(160<gval&&gval<=170)
{
pwmWrite(PWM_PIN_SPEED,900);
pwmWrite(PWM_PIN_CHANGE,900);
}
if(170<gval&&gval<=180)
{
pwmWrite(PWM_PIN_SPEED,950);
pwmWrite(PWM_PIN_CHANGE,950);
}
if(180<gval&&gval<=190)
{
pwmWrite(PWM_PIN_SPEED,1000);
pwmWrite(PWM_PIN_CHANGE,1000);
}
if(190<gval&&gval<=200)
{
pwmWrite(PWM_PIN_SPEED,1000);
pwmWrite(PWM_PIN_CHANGE,1000);
}
}
void ship_mode_3_left(unsigned char gval)
{
digitalWrite(5, LOW);
digitalWrite(7, LOW);
}
void ship_mode_4_right(unsigned char gval)
{
digitalWrite(24, LOW);
digitalWrite(26, LOW);
}
void ship_speed_change(unsigned char *buf)//船速度和转向引脚数值处理函数
{
unsigned char mode=buf[1];
unsigned char val=buf[2];
switch(mode)
{
case 1:
ship_mode_1_flont(val);
break;
case 2:
ship_mode_2_back(val);
break;
case 3:
ship_mode_3_left(val);
break;
case 4:
ship_mode_4_right(val);
break;
default:
break;
}
}
No preview for this file type
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <ifaddrs.h>
#include "ip.h" #include "ip.h"
struct ifaddrs *ifap, *ifa; struct ifaddrs *ifap, *ifa;
struct sockaddr_in *sa; struct sockaddr_in *sa;
char ip_address[INET_ADDRSTRLEN]; char ip_address[INET_ADDRSTRLEN];
......
No preview for this file type
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <mosquitto.h>
#include <cjson/cJSON.h>
#include<time.h>
#include <pthread.h>
#include "ip.h"
#include "mqtt.h" #include "mqtt.h"
#include "opensh.h"
#include "gpio_pwm.h"
#include "INA226.h"
char* TOPIC; char* TOPIC;
char* TOPIC2; char* TOPIC2;
......
No preview for this file type
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <mosquitto.h>
#include <cjson/cJSON.h>
#include<time.h>
#include <pthread.h>
#include <stdint.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include "ip.h"
#include "mqtt.h"
#include "opensh.h" #include "opensh.h"
...@@ -37,8 +24,6 @@ int opencamsh() ...@@ -37,8 +24,6 @@ int opencamsh()
void refresh_cam() {//刷新页面 void refresh_cam() {//刷新页面
setenv("XAUTHORITY", "/home/orangepi/.Xauthority", 1);//加入授权 setenv("XAUTHORITY", "/home/orangepi/.Xauthority", 1);//加入授权
const char *search_command = "xdotool search --class \"chromium-browser\"";//获取窗口id const char *search_command = "xdotool search --class \"chromium-browser\"";//获取窗口id
FILE *fp = popen(search_command, "r"); FILE *fp = popen(search_command, "r");
......
No preview for this file type
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <fcntl.h>
#include <pthread.h>
#include <unistd.h>
#include <termios.h>
#include <mosquitto.h>
#include <cjson/cJSON.h>
#include <time.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <ifaddrs.h>
#include <wiringPi.h>
#include "gpio_pwm.h"
#include "main.h" #include "main.h"
#include "ip.h"
#include "mqtt.h"
#include "delay.h"
#include "opensh.h"
#include "INA226.h"
char buffer[30]; // 用于存储文件内容 char buffer[30]; // 用于存储文件内容
uint8_t AppExit_pin_pwm=0;//判断坦克或者车的退出 uint8_t AppExit_pin_pwm=0;//判断坦克或者车的退出
...@@ -46,7 +24,11 @@ void *AppExit(void *arg)//出现意外自动停止 ...@@ -46,7 +24,11 @@ void *AppExit(void *arg)//出现意外自动停止
pin_all_default(); pin_all_default();
digitalWrite(2, LOW); digitalWrite(2, LOW);
digitalWrite(21, LOW); digitalWrite(21, LOW);
}
if(AppExit_pin_pwm==3)//船异常问题处理
{
ship_stop_pwm();
pin_all_ship_default();
} }
gPwmCount=6; gPwmCount=6;
} }
...@@ -81,6 +63,7 @@ void *opensh(void *arg) ...@@ -81,6 +63,7 @@ void *opensh(void *arg)
Delay_Ms(5,0); Delay_Ms(5,0);
printf("open cam\n"); printf("open cam\n");
opencamsh();//10s后打开游览器并且进入网址 opencamsh();//10s后打开游览器并且进入网址
return NULL; return NULL;
} }
...@@ -150,19 +133,25 @@ int main(int argc, char *argv[]) { ...@@ -150,19 +133,25 @@ int main(int argc, char *argv[]) {
printf("开始初始化了"); printf("开始初始化了");
if(strcmp(sub_str,"02")==0){//坦克的编码 if(strcmp(sub_str,"02")==0){//坦克的编码
pin_init(); pin_init();
pinMode(2, OUTPUT); pinMode(2, OUTPUT);//pwm引脚改为普通引脚
pinMode(21, OUTPUT); pinMode(21, OUTPUT);//pwm引脚改为普通引脚
digitalWrite(2, LOW); digitalWrite(2, LOW);
digitalWrite(21, LOW); digitalWrite(21, LOW);
AppExit_pin_pwm=1;//坦克的异常停止值 AppExit_pin_pwm=1;//坦克的异常停止值
free(sub_str); free(sub_str);
}else if(strcmp(sub_str,"01")==0){//车的编码 }else if(strcmp(sub_str,"01")==0){//车的编码
pwm_speed(); //pwm初始化,车为停止 pwm_speed(); //pwm初始化,车为停止
pin_init(); pin_init();
AppExit_pin_pwm=2;//车的异常停止值 AppExit_pin_pwm=2;//车的异常停止值
free(sub_str); free(sub_str);
}else if(strcmp(sub_str,"03")==0){//船的编码
pwm_speed();
pin_ship_init(); //pwm初始化,车为停止
AppExit_pin_pwm=3;//车的异常停止值
free(sub_str);
} }
Delay_Ms(20,0); Delay_Ms(20,0);
TOPIC=malloc(24); TOPIC=malloc(24);
TOPIC2=malloc(24); TOPIC2=malloc(24);
......
No preview for this file type
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment