int main()
{
// SET single LED port
DDRC |= _BV(LED_PIN);
PORTC ^= _BV(LED_PIN);
// 1s pull up of LED Strip pins */
apa102_DDRREG &= ~_BV(apa102_data);
apa102_DDRREG &= ~_BV(apa102_clk);
apa102_PORTREG |= _BV(apa102_data);
apa102_PORTREG |= _BV(apa102_clk);
_delay_ms(900);
// Disable pull ups
apa102_PORTREG &= ~_BV(apa102_data);
apa102_PORTREG &= ~_BV(apa102_clk);
apa102_DDRREG |= _BV(apa102_data);
apa102_DDRREG |= _BV(apa102_clk);
_delay_ms(100);
PORTC ^= _BV(LED_PIN);
// Clear LEDs in the strip
clear_all_leds();
// Init ROS
nh.initNode();
nh.subscribe(set_sub);
nh.subscribe(set_led_sub);
ack_led();
// Wait for Server side to start
while (!nh.connected())
{
nh.spinOnce();
// LUFA functions that need to be called frequently to keep USB alive
CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
USB_USBTask();
_delay_ms(10);
}
ack_led();
// Publish some debug information
snprintf(log_str, MAX_MSG_SIZE, "V:%s", GIT_VERSION);
nh.loginfo(log_str);
snprintf(log_str, MAX_MSG_SIZE, "FM:%d", get_free_ram());
nh.loginfo(log_str);
while(1)
{
nh.spinOnce();
// LUFA functions that need to be called frequently to keep USB alive
CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
USB_USBTask();
}
return 0;
}
/**
* @brief provides imu readings in a 50 Hz rate.
*
*/
void loop() {
if((millis()-timer)>=20) { // Main loop runs at 50Hz
timer=millis();
//Read data from the hardware
gyro.read();
compass.readAcc();
compass.readMag();
//Assign read data to the ros messages
imu_msg.angular_velocity.x=gyro.g.x;
imu_msg.angular_velocity.y=gyro.g.y;
imu_msg.angular_velocity.z=gyro.g.z;
imu_msg.linear_acceleration.x=compass.a.x;
imu_msg.linear_acceleration.y=compass.a.y;
imu_msg.linear_acceleration.z=compass.a.z;
mag_msg.magnetic_field.x=compass.m.x;
mag_msg.magnetic_field.y=compass.m.y;
mag_msg.magnetic_field.z=compass.m.z;
//Publish the data to the ros message system
imu_pub.publish( &imu_msg );
mag_pub.publish( &mag_msg);
nh.spinOnce();
}
nh.spinOnce();
}
void setup() {
nh.initNode();
//Serial.begin(38400);
delay(1000);
rlog = new RosLogger(nh);
// quadratureEncoder = new QuadratureEncoder();
// lineSensor = new LineSensor();
// lineSensor->calibrate();
motor = new Motor(nh);
Motor::Command c;
//c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::BACKWARD; motor->enqueue(c);
c.direction = Motor::FORWARD; motor->enqueue(c);
// c.direction = Motor::STOP; motor->enqueue(c);
// c.direction = Motor::RIGHT_TURN; motor->enqueue(c);
// c.direction = Motor::STOP; motor->enqueue(c);
// c.direction = Motor::BACKWARD; motor->enqueue(c);
// c.direction = Motor::STOP; motor->enqueue(c);
// c.direction = Motor::LEFT_TURN; motor->enqueue(c);
// c.direction = Motor::STOP; motor->enqueue(c);
for (int i = 0; i < 2; i++) {
nh.spinOnce();
delay(1000);
}
for (int i = 0; i < 4; i++) {
rlog->info("START UP %d", i);
nh.spinOnce();
delay(200);
}
}
int main() {
t.start();
nh.initNode();
nh.advertise(pub_temp);
long publisher_timer =0;
while (1) {
if (t.read_ms() > publisher_timer) {
// step 1: request reading from sensor
//Wire.requestFrom(sensorAddress,2);
char cmd = 2;
i2c.write(sensorAddress, &cmd, 1);
wait_ms(50);
char msb;
char lsb;
int temperature;
i2c.read(sensorAddress, &msb, 1); // receive high byte (full degrees)
i2c.read(sensorAddress, &lsb, 1); // receive low byte (fraction degrees)
temperature = ((msb) << 4); // MSB
temperature |= (lsb >> 4); // LSB
temp_msg.data = temperature*0.0625;
pub_temp.publish(&temp_msg);
publisher_timer = t.read_ms() + 1000;
}
nh.spinOnce();
}
int main()
{
volatile unsigned int d;
/* initialize ROS & subscribers & publishers */
//nh.initNode();
nh.initNode(rosSrvrIp);
nh.advertise(sonar1); // advertise sonar range topic
nh.subscribe(motorSub); // subscribe to motor speed topic
nh.subscribe(servoSub); // subscribe to servo position
// reset bit 0, set as output for sonar trigger
gpio.SetData(0x0000);
gpio.SetDataDirection(0x0001);
// set callbacks on negative edge for both bits 0 (trigger)
// and 1 (echo)
gpio.RegisterCallback(0, NULL, callback);
gpio.RegisterCallback(1, NULL, callback);
gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);
// trigger sonar by toggling bit 0
while(1)
{
gpio.SetData(0x0001);
for (d=0; d<120000; d++);
gpio.SetData(0x0000);
usleep(100000); // the interrupt breaks us out of this sleep
usleep(100000); // now really sleep
sonar1.publish( &range );
nh.spinOnce();
}
}
int main()
{
int error = 0;
t.start();
nh.initNode();
nh.advertise(pub_temp);
nh.advertise(pub_humidity);
long publisher_timer = 0;
temp_msg.header.frame_id = "/base_link";
humidity_msg.header.frame_id = "/base_link";
while (1)
{
if (t.read_ms() > publisher_timer)
{
error = sensor.readData();
if (0 == error)
{
temp_msg.temperature = sensor.ReadTemperature(CELCIUS);
temp_msg.header.stamp = nh.now();
pub_temp.publish(&temp_msg);
humidity_msg.relative_humidity = sensor.ReadHumidity();
humidity_msg.header.stamp = nh.now();
pub_humidity.publish(&humidity_msg);
}
publisher_timer = t.read_ms() + 1000;
}
nh.spinOnce();
}
}
int main(void)
{
// TivaC application specific code
MAP_FPUEnable();
MAP_FPULazyStackingEnable();
// TivaC system clock configuration. Set to 80MHz.
MAP_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
uint8_t button_debounced_delta;
uint8_t button_raw_state;
ButtonsInit();
// ROS nodehandle initialization and topic registration
nh.initNode();
nh.advertise(button_publisher);
while (1)
{
uint8_t button_debounced_state = ButtonsPoll(&button_debounced_delta, &button_raw_state);
// Publish message to be transmitted.
button_msg.sw1.data = button_debounced_state & LEFT_BUTTON;
button_msg.sw2.data = button_debounced_state & RIGHT_BUTTON;
button_publisher.publish(&button_msg);
// Handle all communications and callbacks.
nh.spinOnce();
// Delay for a bit.
nh.getHardware()->delay(100);
}
}
void loop(){
unsigned long pulse_length = pulseIn(PAUSE_PIN, HIGH);
bool_msg.data = pulse_length>1400;//will not pause if pulseIn timed out (no pulse = 0)
pause_pub.publish( &bool_msg );
//if(nh.connected()){
if( !bool_msg.data)
{
unsigned long time = millis();
if(time-last_invert>=(1000/1/2)){
digitalWrite(LIGHT_PIN, !digitalRead(LIGHT_PIN));
last_invert = time;
}
}
// }
else
{
digitalWrite(LIGHT_PIN, LIGHT_ON);
}
// else
// digitalWrite(LIGHT_PIN, LIGHT_ON);
delay(50);
nh.spinOnce();
}
// Note: connector labeled "INPUT" on sonar sensor goes to
// digital 1 (bit 0), and connector labeled "OUTPUT" goes to
// digital 2 (bit 1).
int main()
{
CQEGpioInt &gpio = CQEGpioInt::GetRef();
volatile unsigned int d;
// reset bit 0, set as output for sonar trigger
gpio.SetData(0x0000);
gpio.SetDataDirection(0x0001);
// set callbacks on negative edge for both bits 0 (trigger)
// and 1 (echo)
gpio.RegisterCallback(0, NULL, callback);
gpio.RegisterCallback(1, NULL, callback);
gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);
//nh.initNode();
nh.initNode(rosSrvrIp);
nh.advertise(sonar1);
// trigger sonar by toggling bit 0
while(1)
{
gpio.SetData(0x0001);
for (d=0; d<120000; d++);
gpio.SetData(0x0000);
sleep(1); // the interrupt breaks us out of this sleep
sleep(1); // now really sleep a second
sonar1.publish( &range );
nh.spinOnce();
}
}
int main() {
unsigned long last_pub;
/* set up interrupt handling */
// set up timer interrupts
// fast PWM mode; interrupt and reset when counter equals OCR0A
// prescalar 64
TCCR0A = (1 << WGM01 | 1 << WGM00);
TCCR0B = (1 << WGM02 | 1 << CS01 | 1 << CS00);
// interrupt on "overflow" (counter match)
TIMSK0 = (1 << TOIE0);
OCR0A = 249; // 250 counts per tick
nh.initNode();
nh.advertise(pub);
nh.subscribe(sub);
last_pub = nh.now().toNsec();
while(1) {
nh.spinOnce();
// do our best to publish once per second
if( nh.now().toNsec() - last_pub > 1000000000ull ) {
pub.publish(&msg);
last_pub += 1000000000ull;
}
}
}
void loop()
{
motor_VFL.writeMicroseconds(MotorPWM[MOTOR_VFL-1]);
motor_VFR.writeMicroseconds(MotorPWM[MOTOR_VFR-1]);
motor_VBL.writeMicroseconds(MotorPWM[MOTOR_VBL-1]);
motor_VBR.writeMicroseconds(MotorPWM[MOTOR_VBR-1]);
motor_HFL.writeMicroseconds(MotorPWM[MOTOR_HFL-1]);
motor_HFR.writeMicroseconds(MotorPWM[MOTOR_HFR-1]);
motor_HBL.writeMicroseconds(MotorPWM[MOTOR_HBL-1]);
motor_HBR.writeMicroseconds(MotorPWM[MOTOR_HBR-1]);
sDepth.read();
fDepth.data = -1.0 * sDepth.depth();//Mult by -1 so negative depth is down
pDepth.publish(&fDepth);
chatter.publish(&str_msg);
if (digitalRead(START_IN_PIN) == LOW){
bStart.data = true;
} else {
bStart.data = false;
}
if (digitalRead(STOP_IN_PIN) == LOW){
bStop.data = true;
} else {
bStop.data = false;
}
pStart.publish(&bStart);
pStop.publish(&bStop);
nh.spinOnce();
}
void loop()
{
v = analogRead(analogPinPressureSensor);
voltage.data = v;
ps_voltage.publish(&voltage);
nh.spinOnce();
delay(1);
}
void loop()
{
// send serial packet:
pub.publish( &serial_msg );
nh.spinOnce();
}
void loop()
{
joint_1_position = jointGet(joint_1_address, I2C_COMMAND_JOINT_GET_POSITION);
// Report it to the publisher.
joint_1_position_msg.data = joint_1_position;
joint_1_position_publisher.publish(&joint_1_position_msg);
// Now, check if we have a new setpoint from the listener.
if (joint_1_new_setpoint != joint_1_current_setpoint)
{
jointSet(joint_1_address,
I2C_COMMAND_JOINT_SET_SETPOINT,
joint_1_new_setpoint);
joint_1_current_setpoint = joint_1_new_setpoint;
}
joint_2_position = jointGet(joint_2_address, I2C_COMMAND_JOINT_GET_POSITION);
// Report it to the publisher.
joint_2_position_msg.data = joint_2_position;
joint_2_position_publisher.publish(&joint_2_position_msg);
// Now, check if we have a new setpoint from the listener.
if (joint_2_new_setpoint != joint_2_current_setpoint)
{
jointSet(joint_2_address,
I2C_COMMAND_JOINT_SET_SETPOINT,
joint_2_new_setpoint);
joint_2_current_setpoint = joint_2_new_setpoint;
}
joint_3_position = jointGet(joint_3_address, I2C_COMMAND_JOINT_GET_POSITION);
// Report it to the publisher.
joint_3_position_msg.data = joint_3_position;
joint_3_position_publisher.publish(&joint_3_position_msg);
// Now, check if we have a new setpoint from the listener.
if (joint_3_new_setpoint != joint_3_current_setpoint)
{
jointSet(joint_3_address,
I2C_COMMAND_JOINT_SET_SETPOINT,
joint_3_new_setpoint);
joint_3_current_setpoint = joint_3_new_setpoint;
}
nh.spinOnce();
delay(100);
}
int main() {
nh.initNode();
nh.subscribe(sub);
while (1) {
nh.spinOnce();
wait_ms(1);
}
}
int main()
{
nh.initNode(ROSSRVR_IP);
nh.advertiseService(server);
while(1) {
nh.spinOnce();
sleep(1);
}
}
int main()
{
//nh.initNode();
nh.initNode(rosSrvrIp);
nh.subscribe(sub);
while(1) {
sleep(1);
nh.spinOnce();
}
}
void loop() {
nh.spinOnce();
if (mode_relay_val == false)
turn_off();
else {
analogWrite(motor1Pin, input_vdes_1);
analogWrite(motor2Pin, input_vdes_2);
analogWrite(motor3Pin, input_vdes_3);
analogWrite(motor4Pin, input_vdes_4);
}
}
void loop(){
// do some stuff here - the joy of interrupts is that they take care of themselves
if (mainTimer.check() == true) {
/*
Serial.print("Encoder: "); Serial.print(encoder0Pos);
Serial.print(" unKnown: ");Serial.print(unknownvalue, DEC);
Serial.println("");
*/
nh.spinOnce();
}
}
int main()
{
// Initialize ADC Interrupts
lastTimeInterruptLeft = avr_time_now();
lastTimeInterruptRight = avr_time_now();
out_msg.deltaUmLeft = 0;
out_msg.deltaUmRight = 0;
// Initialize ROS
nh.initNode();
nh.advertise(io_board_out);
ros::Subscriber<io_to_board> sub("to_ioboard", &ioboard_cb);
nh.subscribe(sub);
// Do timed/repeated stuff
uint32_t lastTimeOdometry = 0UL;
uint32_t lastTimeRake = 0UL;
while(1)
{
// Stop engines and raise rake, if last recieved message is older then 2s
// %TODO raise rake
if ((lastTimeMessage != 0) && (avr_time_now() - lastTimeMessage > 1000))
{
lastTimeMessage = 0;
OCR1A = 0x8000;
OCR1B = 0x8000;
}
// Publish odometry all 40ms
if (avr_time_now() - lastTimeOdometry > 40)
{
out_msg.timestamp = avr_time_now();
io_board_out.publish(&out_msg);
out_msg.deltaUmLeft = 0;
out_msg.deltaUmRight = 0;
lastTimeOdometry = avr_time_now();
}
nh.spinOnce();
// LUFA functions that need to be called frequently to keep USB alive
CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
USB_USBTask();
}
return 0;
}
void loop()
{
//publish the adc value every 50 milliseconds
//since it takes that long for the sensor to stablize
if ( millis() >= range_time ){
int r =0;
range_msg.range = getRange_Ultrasound(5);
range_msg.header.stamp = nh.now();
pub_range.publish(&range_msg);
range_time = millis() + 50;
}
nh.spinOnce();
}
msg_t rosserial_sub_thread(void * arg) {
ros::Subscriber<std_msgs::Empty> sub("toggle_led", &msg_cb );
(void) arg;
chRegSetThreadName("rosserial_sub");
nh.initNode();
nh.subscribe(sub);
for (;;) {
nh.spinOnce();
chThdSleepMilliseconds(5);
}
return CH_SUCCESS;
}
void loop()
{
int sensorVal = analogRead(sensorPin);
sensor_val_msg.data = sensorVal;
sensor_val.publish(&sensor_val_msg);
float voltage = (sensorVal / 1024.0) * 5.0;
sensor_voltage_msg.data = voltage;
sensor_voltage.publish(&sensor_voltage_msg);
float temperature = (voltage - 0.5) * 100;
sensor_temp_msg.data = temperature;
sensor_temp.publish(&sensor_temp_msg);
nh.spinOnce();
delay(1000);
}
int main() {
nh.initNode();
nh.advertise(chatter);
while (1) {
str_msg.data = hello;
chatter.publish( &str_msg );
nh.logdebug(debug);
nh.loginfo(info);
nh.logwarn(warn);
nh.logerror(errors);
nh.logfatal(fatal);
nh.spinOnce();
wait_ms(500);
}
}
void loop() {
if(digitalRead(Button) && deadman_activated == false){
deadman_activated = true;
deadman_switch_.data = deadman_activated;
deadman_switch_.header.stamp = nh.now();
switch_pub.publish(&deadman_switch_);
}
else if(!digitalRead(Button) && deadman_activated == true){
deadman_activated = false;
deadman_switch_.data = deadman_activated;
deadman_switch_.header.stamp = nh.now();
switch_pub.publish(&deadman_switch_);
}
nh.spinOnce();
delay(50);
}
/*
* ROS rosserial publisher thread.
*/
msg_t rosserial_pub_thread(void * arg) {
std_msgs::String str_msg;
ros::Publisher pub("chatter", &str_msg);
(void) arg;
chRegSetThreadName("rosserial_pub");
nh.initNode();
nh.advertise(pub);
for (;;) {
char hello[] = "Hello world!";
str_msg.data = hello;
pub.publish(&str_msg);
nh.spinOnce();
chThdSleepMilliseconds(500);
}
return CH_SUCCESS;
}
void loop()
{
// interval
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval) {
previousMillis = currentMillis;
intervalStarted = true;
numIntervals++;
}
// loop
loopSensor();
loopUltra();
// end
intervalStarted = false;
nh.spinOnce();
// not too fast, otherwise buffer overflow
delay(10);
}
int main(void)
{
SetSysClockTo56();
// ROS nodehandle initialization and topic registration
nh.initNode();
nh.advertise(chatter);
// Initialize LED
GPIO_InitTypeDef GPIO_Config;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_Config.GPIO_Pin = GPIO_Pin_5;
GPIO_Config.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Config.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_Config);
GPIO_WriteBit(GPIOB, GPIO_Pin_5, Bit_RESET);
while (1)
{
// Toggle LED
if (GPIO_ReadOutputDataBit(GPIOB, GPIO_Pin_5))
GPIO_WriteBit(GPIOB, GPIO_Pin_5, Bit_RESET);
else
GPIO_WriteBit(GPIOB, GPIO_Pin_5, Bit_SET);
// Publish message to be transmitted.
str_msg.data = hello;
chatter.publish(&str_msg);
// Handle all communications and callbacks.
nh.spinOnce();
// Delay for a bit.
nh.getHardware()->delay(100);
}
}
void loop() {
// lineSensor->read();
// const LineSensor::TSensorArray& values = lineSensor->sensorValues();
// sprintf(buffer, "Position: %d, values: L> %d, %d, %d, %d, %d, %d, %d, %d <R",
// lineSensor->position(),
// values[0],
// values[1],
// values[2],
// values[3],
// values[4],
// values[5],
// values[6],
// values[7]
// );
// rlog->info("Number: %d, quad: %d", loopCounter++, QuadratureEncoder::Counter());
// rlog->info(buffer);
rlog->info("Queue len: %d", motor->queueLength());
//quadratureEncoder->info();
motor->run();
nh.spinOnce();
delay(500);
}
void setup() {
nh.initNode();
last_time = nh.now();
current_time = nh.now();
//Serial.begin(38400);
//#####delay(1000);
rlog = new RosLogger(nh);
//#####quadratureEncoder = new QuadratureEncoder();
// lineSensor = new LineSensor();
// lineSensor->calibrate();
nh.advertise(odom_pub);
motor = new Motor(nh);
/*
Motor::Command c;
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::BACKWARD; motor->enqueue(c);
c.direction = Motor::FORWARD; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::RIGHT_TURN; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::BACKWARD; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::LEFT_TURN; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
*/
nh.subscribe(Motor::sub);
while (!nh.connected()) { nh.spinOnce(); }
rlog->info("START UP...");
}
