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;
}
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();
}
void setup() {
// put your setup code here, to run once:
StepMotor.setSpeed(150);
pinMode(Button, INPUT_PULLUP);
pinMode(Sensor_Down, INPUT_PULLUP);
pinMode(Sensor_Up, INPUT_PULLUP);
pinMode(EnA, OUTPUT);
pinMode(EnB, OUTPUT);
nh.initNode();
nh.subscribe(sub);
nh.advertise(answer_pub);
nh.advertise(switch_pub);
if(digitalRead(Button)){
deadman_activated = true;
deadman_switch_.data = deadman_activated;
deadman_switch_.header.stamp = nh.now();
switch_pub.publish(&deadman_switch_);
}
else{
deadman_activated = false;
deadman_switch_.data = deadman_activated;
deadman_switch_.header.stamp = nh.now();
switch_pub.publish(&deadman_switch_);
}
down();
}
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);
}
}
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();
}
}
void setup()
{
nh.initNode();
pinMode(directionPinEast1, OUTPUT);
pinMode(directionPinEast2, OUTPUT);
pinMode(pwmPinWest, OUTPUT);
pinMode(directionPinWest2, OUTPUT);
pinMode(pwmPinEast, OUTPUT);
pinMode(directionPinWest1, OUTPUT);
pinMode(directionPinSouthSway1, OUTPUT);
pinMode(directionPinSouthSway2, OUTPUT);
pinMode(pwmPinNorthSway, OUTPUT);
pinMode(directionPinNorthSway2, OUTPUT);
pinMode(pwmPinSouthSway, OUTPUT);
pinMode(directionPinNorthSway1, OUTPUT);
pinMode(directionPinSouthUp1, OUTPUT);
pinMode(directionPinSouthUp2, OUTPUT);
pinMode(pwmPinNorthUp, OUTPUT);
pinMode(directionPinNorthUp2, OUTPUT);
pinMode(pwmPinSouthUp, OUTPUT);
pinMode(directionPinNorthUp1, OUTPUT);
nh.subscribe(subPwmForward);
nh.subscribe(subPwmSideward);
nh.subscribe(subPwmUpward);
nh.subscribe(subPwmTurn);
nh.subscribe(subPwmTurnSway);
Serial.begin(57600);
}
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 setup()
{
nh.initNode();
nh.advertise(sensor_val);
nh.advertise(sensor_voltage);
nh.advertise(sensor_temp);
}
/**
* @brief Initializes the gyro/accelerometer and the magnetometer unit of the imu.
* As well as the arduino subsystem
*/
void setup() {
Wire.begin();
delay(1500);
/********/
/* GYRO */
/********/
gyro.init();
gyro.writeReg(L3G_CTRL_REG4, 0x00); // 245 dps scale
gyro.writeReg(L3G_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
//8.75 mdps/LSB
/****************/
/* MAGNETOMETER */
/****************/
compass.init();
compass.enableDefault();
compass.writeReg(LSM303::CTRL2, 0x08); // 4 g scale: AFS = 001
//0.122 mg/LSB
compass.writeReg(LSM303::CTRL5, 0x10); // Magnetometer Low Resolution 50 Hz
//Magnetometer 4 gauss scale : 0.16mgauss/LSB
//ROS-TF base frame of the imu_data
imu_msg.header.frame_id="base_imu_link";
//Register ROS messages
nh.initNode();
nh.advertise(imu_pub);
nh.advertise(mag_pub);
//starting value for the timer
timer=millis();
}
// 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();
}
}
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);
}
}
void init()
{
nh.initNode();
//~ nh.subscribe(sub);
nh.advertiseService(fireService);
nh.advertiseService(cancelService);
nh.advertiseService(manualService);
}
void setup() {
pinMode(13, OUTPUT);
nh.initNode();
nh.subscribe(sub);
servo.attach(9); //attach it to pin 9
}
void setup()
{
RelayShield::begin();
node_handle.getHardware()->setBaud(115200);
node_handle.initNode();
node_handle.subscribe(relay_subscriber);
}
int main()
{
nh.initNode(ROSSRVR_IP);
nh.advertiseService(server);
while(1) {
nh.spinOnce();
sleep(1);
}
}
void setup(){
nh.initNode();
nh.advertise(pause_pub);
nh.subscribe(rate_sub);
pinMode(PAUSE_PIN, INPUT);
digitalWrite(PAUSE_PIN, HIGH);//enable pullup
pinMode(LIGHT_PIN, OUTPUT);
digitalWrite(LIGHT_PIN, LIGHT_ON);
}
int main() {
nh.initNode();
nh.subscribe(sub);
while (1) {
nh.spinOnce();
wait_ms(1);
}
}
int main()
{
//nh.initNode();
nh.initNode(rosSrvrIp);
nh.subscribe(sub);
while(1) {
sleep(1);
nh.spinOnce();
}
}
int main(void)
{
SetSysClockTo56();
// ROS nodehandle initialization and topic registration
nh.initNode();
// Initialize debug 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);
// Start ROS spin task, responsible for handling callbacks and communications
if (spinInitTask(&nh))
{
// Turn on LED on error
GPIO_WriteBit(GPIOB, GPIO_Pin_5, Bit_SET);
while (1);
}
// Register and init publish task
if (publishInitTask(&nh))
{
// Turn on LED on error
GPIO_WriteBit(GPIOB, GPIO_Pin_5, Bit_SET);
while (1);
}
// Register and init subscribe task
if (subscribeInitTask(&nh))
{
// Turn on LED on error
GPIO_WriteBit(GPIOB, GPIO_Pin_5, Bit_SET);
while (1);
}
// Enter scheduler and loop forever
vTaskStartScheduler();
// In case the scheduler returns for some reason,.
while (1)
{
// Turn on LED on error
GPIO_WriteBit(GPIOB, GPIO_Pin_5, Bit_SET);
}
}
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 setup()
{
nh.initNode();
nh.advertise(pub_range);
range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_msg.header.frame_id = frameid;
range_msg.field_of_view = 0.1; // fake
range_msg.min_range = 0.0;
range_msg.max_range = 6.47;
pinMode(8,OUTPUT);
digitalWrite(8, LOW);
}
// +++++++++++++++++++++++++ main loop +++++++++++++++++++++++++++++++++++++++++++
void setup()
{
// init
nh.initNode();
// setup
setupMotor();
setupSensor();
setupServo();
// advertise
nh.advertise(pub_sensor_tracks);
nh.advertise(pub_range);
// subscribe
nh.subscribe(subscriberServo);
nh.subscribe(motor_sub);
}
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 setup() {
nh.initNode();
pinMode(encoder0PinA, INPUT);
digitalWrite(encoder0PinA, HIGH); // turn on pullup resistor
pinMode(encoder0PinB, INPUT);
digitalWrite(encoder0PinB, HIGH); // turn on pullup resistor
attachInterrupt(0, doEncoder, CHANGE); // encoder pin on interrupt 0 - pin 2
attachInterrupt(1, doEncoder, CHANGE);
Serial.begin (115200);
Serial.println("start"); // a personal quirk
}
void setup()
{
pinMode(13, OUTPUT);
// Set up all of the Digital IO pins.
pinMode(pin_leftCutterCheck,INPUT);
pinMode(pin_rightCutterCheck,INPUT);
pinMode(pin_leftCutterControl,OUTPUT);
pinMode(pin_rightCutterControl,OUTPUT);
// Turn off the cutters by default
digitalWrite(pin_leftCutterControl,LOW);
digitalWrite(pin_rightCutterControl,LOW);
// Initialize the rear panel LED outputs
pinMode(pin_ledHigh,OUTPUT);
pinMode(pin_ledMid,OUTPUT);
pinMode(pin_ledLow,OUTPUT);
digitalWrite(pin_ledHigh, LOW);
digitalWrite(pin_ledMid, LOW);
digitalWrite(pin_ledLow, LOW);
temperatureTop.begin();
temperatureBot.begin();
// Make sure we have temperature sensors, if not, set to something
// unreasonable. This would be 0 in Alabama.
if(!temperatureTop.getAddress(topAddress,0))
{
msgStatus.temperature_1 = 0.0;
} else {
temperatureTop.setResolution(topAddress,9);
temperatureTop.setWaitForConversion(false);
temperatureTop.requestTemperatures();
}
if(!temperatureBot.getAddress(botAddress,0))
{
msgStatus.temperature_2 = 0.0;
} else {
temperatureBot.setResolution(botAddress,9);
temperatureBot.setWaitForConversion(false);
temperatureBot.requestTemperatures();
}
nh.initNode();
nh.advertise(status_pub);
nh.advertiseService(cutter_srv);
}
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);
}
}
/*
* 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 setup()
{
motor_VFL.attach(MOTOR_VFL_PIN);
motor_VFR.attach(MOTOR_VFR_PIN);
motor_VBL.attach(MOTOR_VBL_PIN);
motor_VBR.attach(MOTOR_VBR_PIN);
motor_HFL.attach(MOTOR_HFL_PIN);
motor_HFR.attach(MOTOR_HFR_PIN);
motor_HBL.attach(MOTOR_HBL_PIN);
motor_HBR.attach(MOTOR_HBR_PIN);
motor_VFL.writeMicroseconds(STOP_PWM);
motor_VFR.writeMicroseconds(STOP_PWM);
motor_VBL.writeMicroseconds(STOP_PWM);
motor_VBR.writeMicroseconds(STOP_PWM);
motor_HFL.writeMicroseconds(STOP_PWM);
motor_HFR.writeMicroseconds(STOP_PWM);
motor_HBL.writeMicroseconds(STOP_PWM);
motor_HBR.writeMicroseconds(STOP_PWM);
delay(1000);
Wire.begin();
sDepth.init();
sDepth.setFluidDensity(997);
pinMode(START_IN_PIN, INPUT_PULLUP);
pinMode(STOP_IN_PIN, INPUT_PULLUP);
nh.initNode();
nh.advertiseService(server2);
nh.advertise(pDepth);
nh.advertise(pStart);
nh.advertise(pStop);
nh.advertise(chatter);
nh.subscribe(sub);
}
void setup()
{
Wire.begin();
nh.initNode();
nh.subscribe(joint_1_set_setpoint_listener);
nh.advertise(joint_1_position_publisher);
nh.subscribe(joint_2_set_setpoint_listener);
nh.advertise(joint_2_position_publisher);
nh.subscribe(joint_3_set_setpoint_listener);
nh.advertise(joint_3_position_publisher);
joint_1_current_setpoint = 0.0; // Default setpoint
joint_1_new_setpoint = 0.0;
joint_2_current_setpoint = 0.0; // Default setpoint
joint_2_new_setpoint = 0.0;
joint_3_current_setpoint = 0.0; // Default setpoint
joint_3_new_setpoint = 0.0;
}
