空地联防机器人系统：第七课 项目集成总述 简单

一.TT拓展模块的程序

#include <WiFi.h>
#include <WiFiUdp.h> //引用以使用UDP
#include <RMTT_Libs.h>
#include <Wire.h>

const char *ssid = "****";
const char *password = "****";

WiFiUDP Udp;                      //创建UDP对象
unsigned int localUdpPort = 2333; //本地端口号
unsigned int epUdpPort = 8084; //EP控制端端口号
IPAddress ep_IP(192,168,31,51);

RMTT_RGB tt_rgb;  

struct Button {
    const uint8_t PIN;
    uint32_t numberKeyPresses;
    bool pressed;
};

Button button1 = {26, 0, false};


void IRAM_ATTR isr() {
    button1.numberKeyPresses += 1;
    button1.pressed = true;
    tt_rgb.SetRGB(255, 255, 255);
}
void setup()
{
  Serial.begin(115200);
  Serial.println();

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  while (!WiFi.isConnected())
  {
    delay(5000);
    Serial.print(".");
  }
  Serial.println("Connected");
  Serial.print("IP Address:");
  Serial.println(WiFi.localIP());
  
  pinMode(button1.PIN, INPUT_PULLUP);
  attachInterrupt(button1.PIN, isr, RISING);

  tt_rgb.Init();
  tt_rgb.SetRGB(255, 0, 0); 
  
 }

void loop()
{
  if (button1.numberKeyPresses>=3) {        
        button1.numberKeyPresses=0;
        tt_rgb.SetRGB(0, 255, 0); 
        Udp.beginPacket(ep_IP, epUdpPort); //准备发送数据
        Udp.print("hello ep from tt");    //复制数据到发送缓存
        Udp.endPacket();            //发送数据      
    }
   delay(1000);  
}

二.掌控板端程序

#include <MPython.h>
#include <DFRobot_HuskyLens.h>
// 函数声明
void onButtonAPressed();
// 创建对象
DFRobot_HuskyLens huskylens;

// 主程序开始
void setup() {
	mPython.begin();
	buttonA.setPressedCallback(onButtonAPressed);
	huskylens.beginI2CUntilSuccess();
	huskylens.writeAlgorithm(ALGORITHM_OBJECT_CLASSIFICATION);
}
void loop() {
	huskylens.request();
	display.fillInLine(1, 0);
	rgb.write(1, 0x000000);
	rgb.write(2, 0x000000);
	if (huskylens.isAppear(1,HUSKYLENSResultBlock)) {
		display.setCursorLine(1);
		display.printLine("hello，mimi");
		rgb.write(1, 0x00FF00);
		digitalWrite(P0, HIGH);
	}
	else if (huskylens.isAppear(2,HUSKYLENSResultBlock)) {
		display.setCursorLine(1);
		display.printLine("hi，commander");
		rgb.write(2, 0x00FF00);
		digitalWrite(P0, HIGH);
	}
	else {
		digitalWrite(P0, LOW);
	}
	delay(1000);
}

// 事件回调函数
void onButtonAPressed() {
	display.setCursorLine(1);
	display.printLine("测试程序");
	rgb.write(1, 0xFF0000);
	rgb.write(2, 0x0000FF);
	digitalWrite(P0, LOW);
}

三.通过SDK控制EP和TT的python主程序

from socket import *
import time
from robomaster import robot
import robomaster
import threading

#1.EP Object
ep_robot = robot.Robot()
ep_robot.initialize(conn_type="sta")
ep_robot.set_robot_mode(mode=robot.CHASSIS_LEAD)
ep_chassis = ep_robot.chassis
ep_sensor_adaptor = ep_robot.sensor_adaptor

#2.TT Object
robomaster.config.LOCAL_IP_STR = "192.168.31.51"
tl_drone = robot.Drone()
tl_drone.initialize(conn_type='sta')

def controlEP():
    udpSocket = socket(AF_INET, SOCK_DGRAM)
    udpSocket.bind(("192.168.31.51", 8084))
    print("hello world!")
    try:
        while True:
            receiveData = udpSocket.recvfrom(1024)
            print(">%s:%s" % (str(receiveData[1]), str(receiveData[0])))
            if receiveData: break
            time.sleep(0.5)
    except:
        udpSocket.close()
        print("BYBY!!!")

    # 前进 4米
    ep_chassis.move(x=1.5, y=0, z=0, xy_speed=0.7).wait_for_completed()
    # 右转 90度
    ep_chassis.move(x=0, y=0, z=-90, z_speed=45).wait_for_completed()
    # 前进 2.5米
    ep_chassis.move(x=2.5, y=0, z=0, xy_speed=0.7).wait_for_completed()

    time.sleep(3)
    # 获取传感器转接板io电平
    io = ep_sensor_adaptor.get_io(id=1, port=1)
    print("sensor adapter id1-port1 io is {0}".format(io))
    if int(io) == 0:
        ep_robot.play_audio(filename="welcome.wav").wait_for_completed()
        ep_robot.play_audio(filename="welcome.wav").wait_for_completed()
        ep_robot.play_audio(filename="welcome.wav").wait_for_completed()
    else:
        ep_robot.play_audio(filename="alarm.wav").wait_for_completed()

def controlTT():
    tl_flight = tl_drone.flight
    #time.sleep(10)
    # 起飞后降落
    tl_flight.takeoff().wait_for_completed()
    tl_flight.forward(distance=160).wait_for_completed()
    tl_flight.left(distance=350).wait_for_completed()
    tl_flight.forward(distance=500).wait_for_completed()
    tl_flight.right(distance=300).wait_for_completed()
    tl_flight.forward(distance=150).wait_for_completed()
    tl_flight.land().wait_for_completed()
    tl_drone.close()

if __name__=='__main__':
    time.sleep(20)
    p1=threading.Thread(target=controlEP)
    p2=threading.Thread(target=controlTT)
    p1.start()
    p2.start()
    p1.join()
    p2.join()