Ejemplo n.º 1
0
	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;
	}
Ejemplo n.º 2
0
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, &currentTime ,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;
}
Ejemplo n.º 3
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;
}
Ejemplo n.º 4
0
static msg_t meas_thread(void *arg){
    get_sensors();
}
Ejemplo n.º 5
0
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;
    }
}