int main(void)
{
struct heading result;
/*u08 k=0;
u08 b=0;
u08 choice=0;
u08 button=0;*/
u08 ir=0;
u08 i=0;
x = 64;
y = 48;
initialize();
motor_init();
servo_init();
set_motor_power(0,0);
set_motor_power(1,0);
set_motor_power(2,0);
set_motor_power(3,0);
compass_init();
sonar_init();
calCompass();
while(1)
{
clear_screen();
result = compass();
/* use calibration data */
result.x -= compZero.x;
result.y -= compZero.y;
print_string("x "); /* 2 */
print_int(result.x); /* 3 */
print_string(" y "); /* 3 */
print_int(result.y); /* 3 */
/* print_string(" s "); */ /* 3 */
/* print_int(getSonar(0)); */ /* 3 */
/* =17 */
next_line();
/* print_string("a "); */ /* 2 */
/* print_int(IR(0)); */ /* 3 */
/* print_string(" b "); */ /* 3 */
/* print_int(analog(1)); */ /* 3 */
/* print_string(" c "); */ /* 3 */
/* print_int(analog(2)); */ /* 3 */
/* print_int(i++); */ /* =17 */
/* print_int(distance(0));
print_string(" "); */
print_int(sizeof(int));
delay_ms(200);
/* print_int(i);
i=sweep(); */
}
}
int mpu9150_init(mpu9150_t *dev, i2c_t i2c, mpu9150_hw_addr_t hw_addr,
mpu9150_comp_addr_t comp_addr)
{
char temp;
dev->i2c_dev = i2c;
dev->hw_addr = hw_addr;
dev->comp_addr = comp_addr;
dev->conf = DEFAULT_STATUS;
/* Initialize I2C interface */
if (i2c_init_master(dev->i2c_dev, I2C_SPEED_FAST)) {
DEBUG("[Error] I2C device not enabled\n");
return -1;
}
/* Acquire exclusive access */
i2c_acquire(dev->i2c_dev);
/* Reset MPU9150 registers and afterwards wake up the chip */
i2c_write_reg(dev->i2c_dev, dev->hw_addr, MPU9150_PWR_MGMT_1_REG, MPU9150_PWR_RESET);
hwtimer_wait(HWTIMER_TICKS(MPU9150_RESET_SLEEP_US));
i2c_write_reg(dev->i2c_dev, dev->hw_addr, MPU9150_PWR_MGMT_1_REG, MPU9150_PWR_WAKEUP);
/* Release the bus, it is acquired again inside each function */
i2c_release(dev->i2c_dev);
/* Set default full scale ranges and sample rate */
mpu9150_set_gyro_fsr(dev, MPU9150_GYRO_FSR_2000DPS);
mpu9150_set_accel_fsr(dev, MPU9150_ACCEL_FSR_2G);
mpu9150_set_sample_rate(dev, MPU9150_DEFAULT_SAMPLE_RATE);
/* Disable interrupt generation */
i2c_acquire(dev->i2c_dev);
i2c_write_reg(dev->i2c_dev, dev->hw_addr, MPU9150_INT_ENABLE_REG, REG_RESET);
/* Initialize magnetometer */
if (compass_init(dev)) {
i2c_release(dev->i2c_dev);
return -2;
}
/* Release the bus, it is acquired again inside each function */
i2c_release(dev->i2c_dev);
mpu9150_set_compass_sample_rate(dev, 10);
/* Enable all sensors */
i2c_acquire(dev->i2c_dev);
i2c_write_reg(dev->i2c_dev, dev->hw_addr, MPU9150_PWR_MGMT_1_REG, MPU9150_PWR_PLL);
i2c_read_reg(dev->i2c_dev, dev->hw_addr, MPU9150_PWR_MGMT_2_REG, &temp);
temp &= ~(MPU9150_PWR_ACCEL | MPU9150_PWR_GYRO);
i2c_write_reg(dev->i2c_dev, dev->hw_addr, MPU9150_PWR_MGMT_2_REG, temp);
i2c_release(dev->i2c_dev);
hwtimer_wait(HWTIMER_TICKS(MPU9150_PWR_CHANGE_SLEEP_US));
return 0;
}
void warlock_init (void)
{
#ifdef DEBUG
g_log_set_always_fatal (G_LOG_LEVEL_CRITICAL | G_LOG_LEVEL_WARNING);
#endif
preferences_init ();
highlight_init ();
preferences_dialog_init ();
profile_dialog_init ();
text_strings_dialog_init ();
hand_init (warlock_get_widget ("left_hand_label"),
warlock_get_widget ("right_hand_label"));
status_init (warlock_get_widget ("status_table"));
compass_init (warlock_get_widget ("compass_table"));
warlock_entry_init (warlock_get_widget ("warlock_entry"));
macro_init ();
macro_dialog_init ();
warlock_time_init ();
warlock_views_init ();
warlock_log_init ();
script_init ();
}
int main() // main function
{
int x, y, z; // Declare x, y, & z axis variables
i2c *bus = i2c_newbus(3, 2, 0); // New I2C bus SCL=P3, SDA=P2
compass_init(bus); // Initialize compass on bus.
while(1) // Repeat indefinitely
{
compass_read(bus, &x, &y, &z); // Compass vals -> variables
print("%cx=%d, y=%d, z=%d%c\n", // Display compass variables
HOME, x, y, z, CLREOL);
float fy = (float) y; // Ints to floats
float fx = (float) x;
float heading = atan2(fy, fx) * 180.0/PI; // Calculate heading with floats
if(heading < 0.0) // Convert -180...+180 to 0...360
heading = (360.0 + heading);
print("heading = %.2f%c\n", // Display heading
heading, CLREOL);
pause(500); // Wait 1/2 second
}
}
void init_state() {
static int first = 1;
int retval;
if(first) {
if(debug)
spawn_debug_thread();
#ifdef USE_KILL_SWITCH
//initialize kill switch
retval = switch_init();
if(retval != SWITCH_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), SWITCH_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
kill_switch_initialized = 1;
#endif
#ifdef USE_GPS
// initialize gps
retval = gps_init();
if(retval != GPS_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), GPS_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
gps_initialized = 1;
#endif
#ifdef USE_SONAR
// initialize sonar sensors
retval = sonar_init();
if(retval != SONAR_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), SONAR_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
sonar_initialized = 1;
#endif
#ifdef USE_COMPASS
//initialize compass
retval = compass_init();
if(retval != COMPASS_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), SONAR_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
compass_initialized = 1;
#endif
#ifdef USE_CAMERA
//initialize camera
retval = camera_init();
if(retval != CAMERA_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), CAMERA_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
retval = camera_start_tracking();
if(retval != CAMERA_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), CAMERA_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
camera_initialized = 1;
#endif
#ifdef USE_CAR
retval = init_car();
if(retval != CAR_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), CAR_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
car_initialized = 1;
#endif
spawn_device_threads();
first = 0;
}
if (kill_switch_asserted)
next_state = PAUSE_STATE;
else
next_state = NAVIGATION_STATE;
//next_state = TRACK_STATE;
}
int main(void) {
_delay_ms(100);
// IO port initialisation
port_direction_init();
centre.x = ((float)LCD_WIDTH-1.0)/(float)2.0; //the X origin offset
centre.y = ((float)LCD_HEIGHT-1.0)/(float)2.0; //the Y origin offset
//set up the compass serial port
compass_init();
LCD_init();
SREG |= 0x80; //Set the global interrupt enable bit in the Status Register SREG, see section 6.3.1 of the datasheet.
LCD_clear();
//north
fixedPointLine northline1;
northline1.start.x = fip(centre.x);
northline1.start.y = fip(0);
northline1.end.x = fip(37);
northline1.end.y = fip(centre.y);
fixedPointLine northline2;
northline2.start.x = fip(centre.x);
northline2.start.y = fip(0);
northline2.end.x = fip(46);
northline2.end.y = fip(centre.y);
//north small
fixedPointLine northsmallline1;
northsmallline1.start.x = fip(41.5);
northsmallline1.start.y = fip(15);
northsmallline1.end.x = fip(46);
northsmallline1.end.y = fip(23.5);
fixedPointLine northsmallline2;
northsmallline2.start.x = fip(41.5);
northsmallline2.start.y = fip(15);
northsmallline2.end.x = fip(37);
northsmallline2.end.y = fip(23.5);
//horizontal line
fixedPointLine horizontalline;
horizontalline.start.x = fip(30);//37;
horizontalline.start.y = fip(centre.y);
horizontalline.end.x = fip(53);//46;
horizontalline.end.y = fip(centre.y);
//south
fixedPointLine southline1;
southline1.start.x = fip(centre.x);
southline1.start.y = fip(47);
southline1.end.x = fip(46);
southline1.end.y = fip(centre.y);
fixedPointLine southline2;
southline2.start.x = fip(centre.x);
southline2.start.y = fip(47);
southline2.end.x = fip(37);
southline2.end.y = fip(centre.y);
fixedPointPolygon northTriangle;
northTriangle.num_vertices = 4;
fixedPointCoord vertices[northTriangle.num_vertices];
vertices[0].x = fip(centre.x); //point
vertices[0].y = fip(0);
vertices[1].x = fip(46); //bottom right
vertices[1].y = fip(centre.y);
vertices[2].x = fip(centre.x); //middle arrow
vertices[2].y = fip(19);
vertices[3].x = fip(37); //bottom left
vertices[3].y = fip(centre.y);
northTriangle.vertices = vertices; //vertices points to the start of the array
fixedPointPolygon rotatedPoly;
fixedPointEdgeTable globalEdgeTable;
while (1) {
//TODO: use a timed interrupt to send the LCD update
//TODO: stop the LCD update during the compass update (MAYBE??)
//TODO: name public and private headers
//TODO: separate out LCD functions
updated = 0;
compass_transmit((char) 0x12);
while (!updated) {
}
LCD_clear_buff();
//north
//draw_line_l(rotateLine(northline1, angle));
//draw_line_l(rotateLine(northline2, angle));
//north small
/*draw_line_l(rotateLine(northsmallline1, angle));
draw_line_l(rotateLine(northsmallline2, angle));*/
//horizontal line
//draw_line_l(rotateLine(horizontalline, angle));
//south
draw_line_l(rotateLine(southline1, angle));
if (rotatePolygon(&northTriangle, angle, &rotatedPoly)) {
if (initialise_global_edge_table(&rotatedPoly, &globalEdgeTable)) {
//remember a logical check that after initialisation, there are at least 2 edges!
draw_polygon(&globalEdgeTable);
free(globalEdgeTable.edges);
globalEdgeTable.edges = NULL;
draw_line_l(rotateLine(southline2, angle));
}
free(rotatedPoly.vertices);
rotatedPoly.vertices = NULL;
}
//steps:
//rotate polygon - check
//http://www.cs.rit.edu/~icss571/filling/how_to.html
//initializing should return a bool for successful or not (cos of mallocing)
//initialize edges
//initializing global edge table (sorted) (size should be total number of edges)
//initializing parity
//initializing the scan-line
//initializing active edge table (size should be number of edges)
//fill polygon
//aim for a draw_scene function (pass an array of polygons and an angle)
// if N is 2 for a polygon, cast its array to a line
LCD_update();
}
}
/* MotorControl running in independent cog */
void motorControl(void *par){
float integral_error = 0.0; // Integral error between servos
float left_error = 0.0, right_error = 0.0; // Proportional velocity error values
int left_velClicks = 0, right_velClicks = 0; // Current Left & Right velocity (in clicks)
int left_last = 0, right_last = 0; // Previous Left & Right velocity (in clicks)
int angleDiff = 0; // Diff between current & desired heading
int turnSpeed = 0; // Speed robot should be turning at
init_encoders(); // Set up wheel encoders.
compass_init(0); // Initialize compass on bus.
while(1){
left_velClicks = get_velClicks(0); // Get current left velocity (in clicks)
right_velClicks = get_velClicks(1); // Get current right velocity (in clicks)
switch (mFunc){ // Based on desired motor function
case FORWARD: // Move robot Forward
case BACKWARD: // or Backward.
// integral_error = K_INT * // Integrate Left & Right velocity
// integrate(left_vel, right_vel, des_bias_clicks); // also introduce possible bias.
left_error = K_PRO *
(des_vel_clicks - left_velClicks - integral_error); // Proportional speed adjustment of left servo
right_error = K_PRO *
(des_vel_clicks - right_velClicks + integral_error); // Proportional speed adjustment of right servo
alter_power(left_error, 0); // Alter servo speeds as necessary
alter_power(right_error, 1);
break;
case LEFT: // Rotate Left or
case RIGHT: // Right desired number of degrees.
curHeading = compass_smplHeading(); // Get current heading from compass
angleDiff = compass_diff(curHeading, desHeading); // Diff between cur & Desired heading
if (mFunc == LEFT){ // Rotate Left desired number of degrees.
while(angleDiff > 0){
turnSpeed = abs(angleDiff)/2; // Use half the turn angle as turn speed.
if(turnSpeed < 10) turnSpeed = 10; // Unless the turn angle is less than 10.
servo_set(WHEEL_L_PIN, 1500-turnSpeed); // Rotate Left wheel
servo_set(WHEEL_R_PIN, 1500-turnSpeed); // Rotate Right wheel
pause(8); // Make sure there is at least 8ms between compass readings.
curHeading = compass_smplHeading(); // Get current heading from compass
angleDiff = compass_diff(curHeading, desHeading); // Diff between Current & Desired heading
}
} else {
if ( mFunc == RIGHT){ // Rotate Right desired number of degrees.
while(angleDiff < 0){
turnSpeed = abs(angleDiff)/2; // Use half the turn angle as turn speed.
if(turnSpeed < 10) turnSpeed = 10; // Unless the turn angle is less than 10.
servo_set(WHEEL_L_PIN, 1500+turnSpeed); // Rotate Left wheel
servo_set(WHEEL_R_PIN, 1500+turnSpeed); // Rotate Right wheel
pause(8); // Make sure there is at least 8ms between compass readings.
curHeading = compass_smplHeading(); // Get current heading from compass
angleDiff = compass_diff(curHeading, desHeading); // Diff between Current & Desired heading
}
} else {
mFunc = STOP; // Rotation complete or invalid parameters.
servo_set(WHEEL_L_PIN, 1500); // Force Left servo to stop
servo_set(WHEEL_R_PIN, 1500); // Force Right servo to stop
}
}
break;
case STOP: // Stop servo motion immediately
default:
mFunc = STOP; // Set motor function to stop
servo_set(WHEEL_L_PIN, 1500); // Force Left servo to stop
servo_set(WHEEL_R_PIN, 1500); // Force Right servo to stop
integral = 0.0; // Reset Integral to zero
break;
}
pause(control_interval);
}
}
