seaking1/system_light.cpp at main · PVITSpace/seaking1 · GitHub

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
 #include "system_light.h"
 #include "arduino.h"

CLight::~CLight(){

}

 void CLight::setup(){  //Never use Pin 23
  Name("LIGHT");
  _forever=true;

  _INTERVAL=10;

     CMsg m;
    m.Parameters["SYS"]="IMU";
    //m.Parameters["ACT"]="SPEED";
    //m.Parameters["SPEED"]="1.0";
    //m.Parameters["DURATION"]="50";
    m.Parameters["CALLBACK"]="LIGHT";

    addMessageList(m);

 }


 void CLight::loop(){
float x=_cmsg.getParameter("GYRO_X",0L);
unsigned long msgTime=_cmsg.getParameter("GYRO_T",0L);
if (msgTime==0)
  return;


if (getTime()>msgTime+100)
  return;

  writeconsole("---Speed Dif: ");
  writeconsoleln(getTime()-msgTime);


  if(x>0.01){
    CMsg m;
      m.Parameters["SYS"]="PWM1";
      m.Parameters["ACT"]="SPEED";
      m.Parameters["SPEED"]="1.0";
      m.Parameters["DURATION"]="50";
      //m.Parameters["CALLBACK"]="LIGHT";

      addMessageList(m);
      _cmsg.Parameters["x"]="0.0";

  }

    if(x<-0.01){
    CMsg m;
      m.Parameters["SYS"]="PWM2";
      m.Parameters["ACT"]="SPEED";
      m.Parameters["SPEED"]="1.0";
      m.Parameters["DURATION"]="50";
      //m.Parameters["CALLBACK"]="LIGHT";

      addMessageList(m);
      _cmsg.Parameters["x"]="0.0";

  }
  _cmsg.Parameters["GYRO_T"]="0";  //Shows it is complete so we dont use it again
 }