void goThroughPassage(int speed)
{
sendCommand("C RME");
sendCommand("I LR -25 25");
SensorValue frontInfrareds, ultraSound;
SensorValue *list = NULL;
sensorRead(SensorTypeUS, &ultraSound);
while (ultraSound.values[0] > 30 || 1)
{
sensorRead(SensorTypeIFLR, &frontInfrareds);
sensorRead(SensorTypeUS, &ultraSound);
infraredsToDist(&frontInfrareds, SensorTypeIFLR);
double ratio = (double)frontInfrareds.values[RIGHT] / (double)frontInfrareds.values[LEFT];
driveRobotAndRecord(0.01, speed, ratio, &list);
}
printList(list);
stopMotorsAndWait(1);
playBackRecording(&list, speed);
}
int main(int argc, char ** argv) {
int sensor_state;
char value[ARRAY_SIZE(sensor_dev)];
doorbell_init();
signal(SIGINT, Stop);
printf("you can press ctrl+c to stop\n");
while(!stop) {
sensor_state = sensorRead(devFD, value, ARRAY_SIZE(sensor_dev));
if(value[SOUND] || value[BUTTON]) {
if(doorbell_open() == -1) {
printf("fail to open doorbell\n");
return -1;
}
value[SOUND] = 0;
value[BUTTON] = 0;
usleep(1000*500);
}
if(doorbell_close() == -1) {
printf("fail to close doorbell\n");
return -1;
}
}
doorbell_deinit();
return 0;
}
void *imuUpdate(void *arg) {
//struct timeval t0, t1;
//float elapsed;
struct sched_param param = {.sched_priority = 15};
sched_setscheduler(0, SCHED_RR, ¶m);
prctl(PR_SET_NAME,"imu",0,0,0);
while (1) {
sem_wait(&sem_imu_trigger);
//gettimeofday(&t0, NULL);
pthread_mutex_lock(&_imu_lock);
sensorRead(&_imu_working_reg); // read 9-dof sensor
//gettimeofday(&t1, NULL);
//elapsed = (t1.tv_sec - t0.tv_sec)*1000 + (t1.tv_usec - t0.tv_usec) / 1000.0f;
//printf("Sensor read time: %f ms\n", elapsed);
AHRSupdate(&_imu_working_reg); // call AHRS update routine
getYawPitchRoll(&_imu_working_reg); // return RPY representation
pthread_mutex_unlock(&_imu_lock);
}
}
void jarvisNodeTestApp::connectNodeSignals(sJarvisNode* node)
{
connect(node,SIGNAL(tx()),&m_rxWidget,SLOT(tx()));
connect(node,SIGNAL(rx()),&m_rxWidget,SLOT(rx()));
connect(node,SIGNAL(rawInput(QByteArray)),this,SLOT(console_log(QByteArray)));
connect(node,SIGNAL(writeData(QByteArray)),this,SLOT(console_log(QByteArray)));
connect(node,SIGNAL(incomingEvent(QString,jarvisEvents,QStringList)),this,SLOT(eventLog(QString,jarvisEvents,QStringList)));
//connect(node,SIGNAL(newComponent(sJarvisNodeComponent*)),this,SLOT(addComponent(sJarvisNodeComponent*)));
//connect(&m_sensorsTimer,SIGNAL(timeout()),this,SLOT(timedCmd()));
connect(node,SIGNAL(sensorReads(QVector<QString>,QVector<double>)),this,SLOT(sensorRead(QVector<QString>,QVector<double>)));
connect(node,SIGNAL(ready()),this,SLOT(nodeConnected()));
connect(node,SIGNAL(disconnected()),this,SLOT(nodeDisconnected()));
connect(ui->sliderUpdateInterval,SIGNAL(sliderMoved(int)),node,SLOT(setUpdateInterval(int)));
connect(ui->btnReset,SIGNAL(clicked()),node,SLOT(resetNode()));
}
ProximitySocket::ProximitySocket(QObject *parent) :
_isEnabled(false),
_callActive(false),
_proxClose(false),
QObject(parent)
{
sensor = new QProximitySensor(this);
connect(sensor, SIGNAL(readingChanged()), this, SLOT(sensorRead()));
QDBusConnection::systemBus().connect("", "", "org.ofono.VoiceCall",
"PropertyChanged", this, SIGNAL(ofonoPropertyChanged(QString,QDBusVariant)));
QDBusConnection::sessionBus().registerService(SERVICE_NAME);
QDBusConnection::sessionBus().registerObject(OBJECT_NAME,
this,
QDBusConnection::ExportAllSignals | QDBusConnection::ExportAllSlots);
}
int main(void)
{
int i;
int retSize = -1;
char value[ARRAY_SIZE(lean)];
int devFD = -1;
if ((devFD =sensorInit(lean, ARRAY_SIZE(lean))) == -1) {
printf("Fail to init sensor\n");
return -1;
}
if (( retSize = sensorRead(devFD, value, ARRAY_SIZE(lean)) ) == -1) {
printf("Fail to read sensors\n");
}
if (retSize > 0) {
i = 0;
for(i=0; i<retSize; i++)
{
printf("lean:%d\n", value[i]);
}
}
sensorDeinit(devFD);
return 0;
}
int main()
{
DDRB |= 1<<LED_PIN; // led pin as output
PORTB |= 1<<SWITCH_PIN; // pullup for switch
LED_OFF;
_delay_ms(2);
initPtr(); // cherche le point d'écriture dans le tampon
if(ACQ_MODE) {
int cnt = 0;
int seconds = LOG_INTERVAL - 5; // first mesurement: 5 seconds after power on
write_record(0x7F00, 0); // place a power on mark
for(;;) {
if(ACQ_MODE) {
_delay_ms(20); // TODO work with timer interrupt
if(cnt++>50) { // augmente le compteur de secondes
seconds += 1;
cnt = 0;
}
if(seconds>=LOG_INTERVAL) {
if(sensorRead()) {
write_record(sensor_bytes[0]<<8 | sensor_bytes[1], sensor_bytes[2]<<8 | sensor_bytes[3]);
// clear le compteur de seconde uniquement en cas de succès de lecture (ceci permet de tolérer des échec de lecture du capteur)
seconds = 0;
}
}
// Led clignotante pour signaler le mode aquisition
if((cnt%25)>22) LED_ON;
else LED_OFF;
}
else {
LED_OFF;
}
}
}
else {
#if defined(__AVR_ATtiny85__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny25__)
uint8_t calibrationValue = eeprom_read_byte(0); /* calibration value from last time */
if (calibrationValue != 0xFF)
{
OSCCAL = calibrationValue;
}
#endif
stdout = &mystdout; // set default stream
// initialize report (I never assume it's initialized to 0 automatically)
keyboard_report_reset();
wdt_disable(); // disable watchdog, good habit if you don't use it
// enforce USB re-enumeration by pretending to disconnect and reconnect
usbDeviceDisconnect();
_delay_ms(250);
usbDeviceConnect();
// initialize various modules
usbInit();
sei(); // enable interrupts
while (1) // main loop, do forever
{
if (blink_count > 2) // activated by blinking lights
{
#if !defined(__AVR_ATtiny85__) && !defined(__AVR_ATtiny45__) && !defined(__AVR_ATtiny25__)
DDRD |= _BV(1); // LED lights for debug
PORTD |= _BV(1);
#endif
puts_P(PSTR("----------------------------------"));
printf("LogStick 1.0 interval %d seconds\n", LOG_INTERVAL);
if(ACQ_MODE) printf("Ptr %x\n", ptr);
puts_P(PSTR("----------------------------------"));
// PLACE TEXT HERE
//puts_P(PSTR(" ")); // test size
int humidity;
int temperature;
int p = ptr; // pointeur de la prochaine écriture en mode ACQ
for(;;) {
p -= 4;
if(p<START_PTR) p = MAX_PTR-4;
if(p==ptr) break;
humidity = eeprom_read_word(p);
temperature = eeprom_read_word(p+2);
if(humidity==0x7F00) {
if(ACQ_MODE) printf("%x:", p);
puts_P(PSTR("---POWER ON---"));
}
else if(humidity==0x7FFF) {
puts_P(PSTR("---PTR MARK---")); // ne devrait pas se produire
}
else {
if(ACQ_MODE) printf("%x:%d.%d,%d.%d\n", p, humidity/10, humidity%10, temperature/10, temperature%10);
else printf("%d.%d,%d.%d\n", humidity/10, humidity%10, temperature/10, temperature%10);
}
}
puts_P(PSTR("------------------------------END-"));
blink_count = 0; // reset
}
// perform usb related background tasks
usbPoll(); // this needs to be called at least once every 10 ms
// this is also called in send_report_once
}
}
return 0;
}
