agent_sensor_list agent_body::get_mapped_inputs(std::vector< std::string > const& named_inputs) const
{
auto named_sensors = get_sensors();
agent_sensor_list sensor_ids;
for(auto const& name: named_inputs)
{
auto it = std::find(std::begin(named_sensors), std::end(named_sensors), name);
assert(it != std::end(named_sensors));
agent_sensor_id id = it - std::begin(named_sensors);
sensor_ids.push_back(id);
}
std::sort(std::begin(sensor_ids), std::end(sensor_ids), std::less<>());
return sensor_ids;
}
int get_command(pState state){
//magic len command subcommand data checkval
//message is len + 8 (2 magic, 2 len, 4 checkval)
//magic = 0xa55a, short len, short command, var subcommand, var data, int checkval
// unsigned char *buf = malloc(MAX_MESSAGE_LEN+8);
unsigned char buf[MAX_MESSAGE_LEN+8];
unsigned int checkval = 0;
unsigned int calc_checkval = 0;
unsigned short magic = 0;
unsigned short len = 0;
unsigned short command = 0xaa;
unsigned short subcommand = 0xffff;
unsigned short sensorID = 0xffff;
unsigned int sensor_address = 0xffff;
unsigned int coefficient = 0xffff;
unsigned short temp = 0xffff;
unsigned short numSteps = 0xffff;
unsigned short *tempP = NULL;
unsigned short seconds = 0;
pStep steps = NULL;
int ret = -1;
unsigned int fw = 0;
bzero(buf,MAX_MESSAGE_LEN+8);
//get magic
get_bytes(buf ,2);
magic = get_short(buf);
if (magic != MAGIC){
prime_buf(buf);
send(buf,12);
//free(buf);
return 1;
}
//get len
get_bytes(buf+2,2);
len = get_short(buf+2);
//check len
if (len > MAX_MESSAGE_LEN){
prime_buf(buf);
send(buf,12);
//free(buf);
return 1;
}
//get the rest of message except checkval
get_bytes(buf+4, len);
//get checkval
checkval = get_int(buf+len);
//compare to calculated from message
calc_checkval = check_val( buf, len);
if ( checkval != calc_checkval ){
//bad check_val
prime_buf(buf);
send(buf,12);
//free(buf);
return 1;
}
//get command
command = get_short(buf+4);
switch (command) {
case 1:{ //set power state
subcommand = get_short(buf+6);
prime_buf(buf);
if (subcommand == 0x0000){
power_off(state);
}
if (subcommand == 0x0001){
power_on(state);
}
if (subcommand > 0x0001){
//bad subcommand
}
send(buf, 12);
break;
}
case 2:{//set temp
temp = get_short(buf+6);
prime_buf(buf);
if (set_temp(state,temp) == 2){
buf[6] = 1;
}
send(buf, 12);
break;
}
case 3:{ //add sensor
sensorID = get_short(buf+6);
sensor_address = get_int(buf+8);
coefficient = get_int(buf+12);
unsigned int sensorTemp = get_int(buf+16);
//check count < 10 buf[6] = 0x08, sensor_ID is unique/not in use buf[6] = 0x07,
ret = add_sensor(state, sensorID, sensor_address, coefficient, sensorTemp);
prime_buf(buf);
if (ret == 2){buf[6]=0x07;}
if (ret == 3){buf[6]=0x08;}
send(buf, 12);
break;
}
case 4:{ //remove sensor
sensorID = get_short(buf+6);
ret = remove_sensor(state,sensorID);
prime_buf(buf);
if (ret == 1){buf[6]=0x06;}
send(buf, 12);
break;
}
case 5:{ //set smoke sensor
subcommand = get_short(buf+6);
prime_buf(buf);
if (subcommand == 0x0000){
smoke_off(state);
}
if (subcommand == 0x0001){
smoke_on(state);
}
if (subcommand > 0x0001){
//bad subcommand
//no response
}
send(buf, 12);
break;
}
case 6:{ //set program
numSteps = get_short(buf+6);
steps = (pStep)(buf+8) ;
ret = add_steps(state,numSteps,steps);
prime_buf(buf);
if (ret == 3){buf[6]=3;}
if (ret == 2){buf[6]=2;}
if (ret == 1){buf[6]=1;}
send(buf, 12);
break;
}
case 7:{//get program
prime_buf(buf);
unsigned int lenz = 0;
unsigned int program[30];
bzero(program,120);
buf[4]=1;
buf[6]=7;
get_program(state, &lenz, program);
lenz = lenz*3*sizeof(int);
cgc_memcpy(buf+8,&lenz,sizeof(int));
cgc_memcpy(buf+12,program,lenz);
send(buf,lenz + 12);
break;
}
case 8:{//get status
prime_buf(buf);
buf[4]=1;
buf[6]=8;
unsigned int status[6];
int len = 24;
ret = get_status(state,status);
cgc_memcpy(buf+8,&len,sizeof(int));
cgc_memcpy(buf+12,status,24);
send(buf,36);
break;
}
case 9:{//simulate seconds
seconds = get_short(buf+6);
prime_buf(buf);
unsigned int bufsize = 12;
unsigned int histSize = 0;
ret = simulate_seconds(state, seconds);
unsigned int currentTime = state->currentTime;
unsigned int setTemp = state->setTemp;
if (ret == 0 ){
buf[6] = 9;
cgc_memcpy(buf+8, ¤tTime ,sizeof(int));
send(buf,12);
}
if(ret == 2){
buf[4] = 1;
buf[6] = 0xc;
histSize = state->historyPosition*sizeof(int);
unsigned int *pHistoryList = state->history;
unsigned int historyListSize = state->historyPosition;
unsigned int ambientTemp = state->ambientTemp;
new_state(state);
if (historyListSize > 0){
cgc_memcpy(buf+8, &historyListSize, sizeof(historyListSize));
cgc_memcpy(buf+12, pHistoryList, histSize);
bufsize = histSize + 12;
}
cgc_memcpy(buf+bufsize, &ambientTemp, sizeof(unsigned int));
bufsize+=4;
cgc_memcpy(buf+bufsize, &setTemp, sizeof(unsigned int));
bufsize+=4;
send(buf, bufsize);
//new_state(state);
}
break;
}
case 0xa:{ //validate firmware
unsigned int fw = validate_fw(state);
prime_buf(buf);
buf[4]=1;
buf[6]=0xa;
buf[8]=4;
cgc_memcpy(buf+12, &fw,sizeof(int) );
send(buf, 16);
break;
}
case 0xb:{//cgc_read sensor list
prime_buf(buf);
int len = 0;
buf[4]=1;
buf[6]=0xb;
len = state->sensorCount * (sizeof(int)*4);
//buff is filled with sensor bytes
unsigned int sensorList[40*sizeof(int)];
get_sensors(state,sensorList);
cgc_memcpy(buf+8,&len,sizeof(int));
cgc_memcpy(buf+12,sensorList,len);
send(buf, len+12);
break;
}
case 0xc:{//set ambient temp
int ambientTemp = get_signed_int(buf+6);
prime_buf(buf);
if (set_ambient_temp(state,ambientTemp) == 2){
buf[6] = 1;
}
send(buf, 12);
break;
}
case 0xff:{
//free(buf);
exit_normal();
break;
}
default:{
//bad command
prime_buf(buf);
buf[6]=5;
send(buf,12);
break;
}
}
// free(buf);
return 0;
}
// --- loop ---
void loop(){
char Key = kpd.getKey();
switch (alarmState) {
// off - system idle
case off:
set_sirene(false);
goto_on_if_password();
break;
// wait_on - delay before on (delai de sortie maison)
case wait_on:
set_sirene(false);
goto_off_if_password();
if(wait_on_timer.check() == 1){
set_lcd("alarm on", "");
next_alarmState = on;
}
break;
// on - system is running
case on:
set_sirene(false);
if(get_sensors()) {
next_alarmState = detection;
set_lcd("detection", "");
}
goto_off_if_password();
break;
// detection - movement detected
case detection:
set_sirene(false);
send_sms();
goto_off_if_password();
before_sirene_timer.reset();
next_alarmState = before_sirene;
break;
// before_sirene - delay before sirene (delai d'entrée dans maison)
case before_sirene:
set_sirene(false);
if(before_sirene_timer.check() == 1){
ring_sirene_timer.reset();
next_alarmState = ring_sirene;
set_lcd("sirene", "");
}
goto_off_if_password();
break;
// sirene - sirene is crying
case ring_sirene:
set_sirene(true);
goto_off_if_password();
if(ring_sirene_timer.check() == 1){
wait_on_timer.reset();
next_alarmState = wait_on;
set_lcd("wait_on", "");
}
break;
}
alarmState = next_alarmState;
}
static msg_t meas_thread(void *arg){
get_sensors();
}
void do_autonomous(void)
{
if (!autonomous)
return;
get_sensors(); // Sensor sampling code
if ( Sumo.atLeftEdge || Sumo.atRightEdge )
Sumo.state = SURVIVE;
switch ( Sumo.state )
{
case SURVIVE:
if ( Sumo.atLeftEdge && Sumo.atRightEdge )
{
sumo_move(BACKWARD);
delay10ktcy(250);
delay10ktcy(250);
//rotate(180);
Sumo.searchClockwise = TRUE;
}
else if ( Sumo.atLeftEdge )
{
sumo_move(BACKWARD);
delay10ktcy(250);
delay10ktcy(250);
//rotate(-135);
//Add a Delay or create a function rotate(int degrees) instead
Sumo.searchClockwise = TRUE;
}
else if ( Sumo.atRightEdge )
{
sumo_move(BACKWARD);
delay10ktcy(250);
delay10ktcy(250);
//rotate(135);
//Add a Delay or create a function rotate(int degrees) instead
Sumo.searchClockwise = FALSE;
}
Sumo.state = HUNT;
break;
case HUNT:
if ( rangeAverage > mTargetThreshold )
Sumo.state = ATTACK;
else if ( Sumo.searchClockwise )
sumo_move(RIGHT);
//arc(mHuntPower, mHuntFactor);
else
sumo_move(LEFT);
//arc(mHuntPower, -mHuntFactor);
break;
case ATTACK:
sumo_move(FORWARD);
if ( rangeAverage < mTargetThreshold )
{
Sumo.state = HUNT;
}
break;
default:
Sumo.state = SURVIVE;
}
}