DF C4002 毫米波人体存在传感器在机器人上的试用

DF C4002 毫米波人体存在传感器是一款基于24GHz FMCW技术的毫米波雷达模块，专为智能家居场景中需要精准静态人体存在感知的应用而设计。模块突破了传统PIR传感器只能检测大幅运动的局限，可在10x10m的有效检测范围内，同步侦测运动人体与静止/微动人体，并支持运动速度检测、运动方向识别（靠近/远离）及环境光检测功能。

当前机器人行业蓬勃发展，特别是人形机器人在各个场景中都呈现高度参与人类生活环境的趋势，但同时由于机器人重量大，各个关节都是电机，与人类接触极易发生风险，经常看到新闻上机器人伤人事件，如果这时靠遥控器或者按急停按钮可能来不及处理。这个安全问题我们可用C4002来解决，通过检测到人体靠近就终止机器人的动作，达到形成自动安全闭环的目的。
用到的硬件：
一个红外遥控机器人、DF C4002 毫米波人体存在传感器、红外发射管、Sparrow 微控制器



Arduino程序：
逻辑：只要C4002获得人体感应，就通过红外管发射终止机器人操作的指令，停止机器人正在进行的动作。


#include "DFRobot_C4002.h"
#if defined(ESP8266) || defined(ARDUINO_AVR_UNO)
SoftwareSerial mySerial(4, 5);
DFRobot_C4002  c4002(&mySerial, 115200);
#elif defined(ESP32)
DFRobot_C4002 c4002(&Serial1, 115200, /*D2*/ D2, /*D3*/ D3);
#else
DFRobot_C4002 c4002(&Serial1, 115200);
#endif
//-------------------buttoncodes-------------------------
#define forward      898819
#define backward     964611
#define sideright    703491
#define sideleft     637699
#define p1           922368
#define k1           991744
#define g1           602624
#define zero         1034752
#define hp           775424

//-------------------info about bits-------------------------------
#define totallength 22            //number of highs/bits 4 channel +18 command
#define channelstart 0
#define commandstart 4       //bit where command starts
#define channellength  4
#define commandlength  18
//---------determined empirically--------------
#define headerlower 2300     //lower limit
#define headernom 2550       //nominal
#define headerupper 2800     //upper limit
#define zerolower 300
#define zeronom 380          //nominal
#define zeroupper 650
#define onelower 800
#define onenom 850          //nominal
#define oneupper 1100
#define highnom 630         
//---------------------pin assignments--------------
#define TXpin A1
#define RXpin 2              //doesnt use interrupts so can be anything
//----------------------variables----------------------
boolean bit2[totallength];
unsigned long buttonnum;






unsigned long power2(int power){    //gives 2 to the (power)
  unsigned long integer=1;          //apparently both bitshifting and pow functions had problems
  for (int i=0; i<power; i++){      //so I made my own
    integer*=2;
  }
  return integer;
}

void ItoB(unsigned long integer, int length){                //needs bit2[length]
  for (int i=0; i<length; i++){
    if ((integer / power2(length-1-i))==1){
      integer-=power2(length-1-i);
      bit2=1;
    }
    else bit2=0;
  }
}

unsigned long BtoI(int start, int numofbits, boolean bit[]){    //binary array to integer conversion
  unsigned long integer=0;
  int i=start;
  int n=0;
  while(n<numofbits){
    integer+=bit*power2((numofbits-n-1)); //same as pow()
    i++;
    n++;
  }
  Serial.println();
  return integer;
}

unsigned long receivecode(){    //receives single code then puts into ulong code
  boolean bit[totallength];      //should be 22 data bits 0-3 are channel code 4-21 are command bits
  unsigned long plen[totallength];  //pulse length for debugging
  int i=0;    //bit number
  boolean channel;//channel A=0 channel B=1
  unsigned long commandcode;  //18bits
  unsigned long totalcode;    //22bits
  
  Serial.println("Waiting...");
  while(digitalRead(RXpin)==HIGH){}//kill time till header pulse comes in
                                   //RX is low here
  for(i=0; i<totallength; i++){
    plen=pulseIn(RXpin, HIGH); //get length of HIGH pulses (yea mine works backwards)
  }
  
  for (i=0; i<totallength; i++){                  //sets bits based on pulselength of HIGHs
    if (plen<zeroupper && plen>zerolower)bit=0;
    else if (plen<oneupper && plen>onelower)bit=1;
  }
  if (bit[0]==0)channel=0;              //calculates channel based on highest bit value
  else if (bit[0]==1)channel=1;
  //commandcode=BtoI(commandstart, commandlength, bit);       //calculates just commandcode
  //totalcode=BtoI(channelstart, totallength, bit);           //calculates totalcode
  /*                                                   //uncomment code for signal analysis
  Serial.println("p len\thi/lo");                //prints pulse length and bit state
  for (i=0; i<totallength; i++){
    Serial.print(plen);
    Serial.print("\t");
    Serial.println(bit, DEC);
  }
  */
  /*
  Serial.print("ch\tb code\t code\n");  //prints channel, buttoncode, channelcode
  if (channel==0)Serial.print("A\t");
  else Serial.print("B\t");
  Serial.print(commandcode);
  Serial.print("\t");
  Serial.println(totalcode);
  */
  /*
  ItoB(commandcode, 18);  //checks ItoB
  ItoB(totalcode, 22);
  for(i=commandstart; i<totallength; i++) Serial.print(bit2, DEC);
  Serial.println();
  for(i=0; i<totallength; i++) Serial.print(bit2, DEC);
  Serial.println();
  */
  for(i=0; i<totallength; i++) plen=0;
  delay(1000);                    //delays to cut repeated
  return(BtoI(channelstart, totallength, bit));
  
}


void oscWrite(int pin, int time) {           //writes at approx 38khz
  for(int i = 0; i < (time / 52) - 1; i++){          //prescaler at 26 for 16mhz, 52 at 8mhz, ? for 20mhz
    digitalWrite(pin, HIGH);
    delayMicroseconds(13);
    digitalWrite(pin, LOW);
    delayMicroseconds(13);
  }
}

void buttonwrite(int txpin, unsigned long integer){      //must be full integer (channel + command)
  ItoB(integer, 22);                               //must have bit2[22] to hold values
  oscWrite(txpin, headernom);
  for(int i=0;i<totallength;i++){
    if (bit2==0)delayMicroseconds(zeronom);
    else delayMicroseconds(onenom);
    oscWrite(txpin, highnom);
  }
  delay(205);
}

//-----------------------program-----------------------
void setup() {
  Serial.begin(38400);
  Serial.println("Analyze Isobot IR Remote");
  pinMode(RXpin, INPUT);
  pinMode(TXpin, OUTPUT);
}
void loop(){
  
  sRetResult_t retResult = c4002.getNoteInfo();
  sPresenceTarget_t presenceTarget = c4002.getPresenceTargetInfo();
  
  if(presenceTarget.distance>0){              
      buttonwrite(TXpin, zero);
      
   
  }
}