[joyos] r292 committed - Undo 291:287
jo...@googlecode.com
Author: vaumnou
Date: Mon Jan 17 22:07:34 2011
Log: Undo 291:287
http://code.google.com/p/joyos/source/detail?r=292
Modified:
/trunk/src/drivers/gyro.c
/trunk/src/drivers/rf.c
/trunk/src/inc/kern/thread.h
/trunk/src/inc/packet.h
/trunk/src/kern/main.c
/trunk/src/kern/thread.c
/trunk/src/lib/pid.c
/trunk/tests/panic_nonhalting.c
/trunk/user/demo.c
/trunk/user/mouse/mouse.c
/trunk/user/umain2.c
=======================================
--- /trunk/src/drivers/gyro.c Mon Jan 17 21:16:52 2011
+++ /trunk/src/drivers/gyro.c Mon Jan 17 22:07:34 2011
@@ -64,7 +64,7 @@
_theta = 0;
init_lock (&gyro_lock, "gyro lock");
- create_thread (&gyro_update, STACK_DEFAULT,
THREAD_PRIORITY_NORMAL, "gyro");
+ create_thread (&gyro_update, STACK_DEFAULT, 0, "gyro");
#endif
}
=======================================
--- /trunk/src/drivers/rf.c Mon Jan 17 21:17:21 2011
+++ /trunk/src/drivers/rf.c Mon Jan 17 22:07:34 2011
@@ -32,6 +32,8 @@
packet_buffer tx, rx;
+volatile board_coord goal_position; //The target position received from a
goal packet
+
FILE rfio = FDEV_SETUP_STREAM(rf_put, rf_get, _FDEV_SETUP_RW);
volatile char rf_str_buf[PAYLOAD_SIZE+1];
@@ -42,8 +44,12 @@
volatile uint8_t rf_new_str;
struct lock rf_lock;
+
+volatile uint8_t light_port = 0xFF;
volatile uint8_t robot_id = 0xFF;
+volatile uint8_t caught[4];
+
void rf_status_update(uint8_t caught){
extern volatile uint8_t robot_id;
@@ -307,6 +313,10 @@
position_microtime = get_time_us();
break;
+ case GOAL:
+ goal_position = rx->payload.coords[0];
+ break;
+
case START:
if (robot_id != 0xFF) {
//Remaining bytes are robots which are starting. Check if
we're one of them.
@@ -332,6 +342,23 @@
memcpy((char *)rf_str_buf, rx->payload.array, PAYLOAD_SIZE);
break;
+ case LIGHT:
+ if (light_port != 0xFF && robot_id != 0xFF) {
+ for (int i=0; i<4; i++) {
+ if (rx->payload.lights[i].id == robot_id)
+ motor_set_vel(light_port,
rx->payload.lights[i].value);
+ }
+ }
+ break;
+
+ case STATUS:
+ for (uint8_t i=0; i<4; i++){
+ if (rx->payload.status.id == objects[i].id){
+ caught[i] = rx->payload.status.caught;
+ break;
+ }
+ }
+ break;
default:
break;
}
@@ -361,22 +388,19 @@
#ifndef SIMULATE
- for(;;) {
- if (spi_try_acquire()) {
- if (!(PINE & _BV(PE7))) {
- uint8_t status = nrf_read_status();
- if (status & _BV(NRF_BIT_RX_DR)) {
- nrf_write_reg(NRF_REG_STATUS, _BV(NRF_BIT_RX_DR));
//reset int
-
- packet_buffer rx;
- uint8_t size;
- uint8_t pipe;
- while ((pipe = rf_get_packet((uint8_t*)&rx, &size)) !=
NRF_RX_P_NO_EMPTY)
- rf_process_packet(&rx, size, pipe);
- }
- }
- spi_release();
- }
+ for (;;) {
+ //if (PINE & _BV(PD7)) {
+ uint8_t status = nrf_read_status();
+ if (status & _BV(NRF_BIT_RX_DR)) {
+ nrf_write_reg(NRF_REG_STATUS, _BV(NRF_BIT_RX_DR)); //reset int
+
+ packet_buffer rx;
+ uint8_t size;
+ uint8_t pipe;
+ while ((pipe = rf_get_packet((uint8_t*)&rx, &size)) !=
NRF_RX_P_NO_EMPTY)
+ rf_process_packet(&rx, size, pipe);
+ }
+ //}
yield();
}
return 0;
@@ -432,5 +456,5 @@
#endif
- create_thread (&rf_receive, STACK_DEFAULT,
THREAD_PRIORITY_REALTIME, "rf");
-}
+ create_thread (&rf_receive, STACK_DEFAULT, 0, "rf");
+}
=======================================
--- /trunk/src/inc/kern/thread.h Mon Jan 17 21:16:52 2011
+++ /trunk/src/inc/kern/thread.h Mon Jan 17 22:07:34 2011
@@ -105,9 +105,6 @@
int (*th_func)();
};
-#define THREAD_PRIORITY_NORMAL 0
-#define THREAD_PRIORITY_REALTIME 1
-
// Macros for extracting jmp_buf thingies
#define JMPBUF_FP_OFFSET 16
=======================================
--- /trunk/src/inc/packet.h Mon Jan 17 21:17:21 2011
+++ /trunk/src/inc/packet.h Mon Jan 17 22:07:34 2011
@@ -10,24 +10,24 @@
#define PAYLOAD_SIZE 30
-enum {
- POSITION, //Updated position of this bot, other bot, or mouse
- START, //Start of the round
- STOP, //End of the round
- STRING, //String from bot to board
- REPLY_STRING //String from board to bot
-} packet_type;
+#define POSITION 0x00 //Updated position of this bot, other bot, or mouse
+#define START 0x01 //Start of the round
+#define STOP 0x02 //End of the round
+#define STRING 0x03 //String from bot to board
+#define SYNC 0x04 //Not sure how to use yet. Established IDs of all
bots/vision system on board
+#define GOAL 0x05 //Sets the target position of this robot
+#define LIGHT 0x06 //Instructs robots to turn their lights on or off
+#define REPLY_STRING 0x07 //String from board to bot
+#define STATUS 0x08 // Status packet from mouse
#endif
typedef struct {
- unsigned id : 8;
+ uint8_t id;
signed x : 12;
signed y : 12;
signed theta : 12;
- signed a : 4;
- signed b : 4;
- unsigned radius : 4;
+ unsigned confidence : 12;
#ifndef SIMULATE
} __attribute__ ((aligned (1))) __attribute__ ((packed)) board_coord;
#else
=======================================
--- /trunk/src/kern/main.c Mon Jan 17 21:16:52 2011
+++ /trunk/src/kern/main.c Mon Jan 17 22:07:34 2011
@@ -87,7 +87,7 @@
#ifndef SIMULATE
init_thread();
- create_thread(&robot_monitor, STACK_DEFAULT,
THREAD_PRIORITY_NORMAL, "main");
+ create_thread(&robot_monitor, STACK_DEFAULT, 0, "main");
rf_init();
schedule();
#else
=======================================
--- /trunk/src/kern/thread.c Mon Jan 17 21:17:11 2011
+++ /trunk/src/kern/thread.c Mon Jan 17 22:07:34 2011
@@ -100,49 +100,26 @@
ATOMIC_END;
}
-
-void resume_if_runnable(int id) {
- if (threads[id].th_status == THREAD_PAUSED &&
- threads[id].th_wakeup_time <= global_time)
- threads[id].th_status = THREAD_RUNNABLE;
-
- if (threads[id].th_status == THREAD_RUNNABLE)
- resume(&threads[id]);
-}
// Schedule a new thread to run
// assume interrupts disabled
void schedule(void) {
- static int id_lo = 0, id_rt = -1;
- static uint32_t last_time = 0;
+ int id = current_thread ? current_thread->th_id : MAX_THREADS-1;
current_thread = NULL;
- if (global_time > last_time) {
- if (id_rt != -1 ||
- ((global_time - last_time) > 1 && last_time != 0))
- panic("Failed to meet realtime commitments");
- last_time = global_time;
- id_rt = 0;
- }
-
- while (id_rt != -1) {
- int tid = id_rt;
- // update id_rt to be the next thread that should be attempted
- id_rt = (id_rt+2) % (MAX_THREADS+1) - 1;
- if (threads[tid].th_priority == THREAD_PRIORITY_REALTIME)
- resume_if_runnable(tid);
+ int offset;
+ for (offset = 1; offset <= MAX_THREADS; offset++) {
+ int tid = (id+offset) % MAX_THREADS;
+
+ if (threads[tid].th_status == THREAD_PAUSED &&
+ threads[tid].th_wakeup_time <= global_time)
+ threads[tid].th_status = THREAD_RUNNABLE;
+
+ if (threads[tid].th_status == THREAD_RUNNABLE)
+ resume(&threads[tid]);
}
- int old_id_lo = id_lo;
- do {
- int tid = id_lo;
- // update id_lo to be the next thread that should be attempted
- id_lo = (id_lo + 1) % MAX_THREADS;
- if (threads[tid].th_priority == THREAD_PRIORITY_NORMAL)
- resume_if_runnable(tid);
- } while (id_lo != old_id_lo);
-
- // wait for the clock to tick
+ // wait for aliens to take us home...
SREG |= SREG_IF;
for(;;);
}
@@ -442,7 +419,6 @@
threads[i].th_runs = 0;
threads[i].th_stacksize = STACKSIZE;
threads[i].th_func = func;
- threads[i].th_priority = priority;
// make the jmp_buf something sensible,
// calculate the thread's stack top, and
@@ -473,7 +449,8 @@
// nuke motor lock
extern struct lock motor_lock;
- smash(&motor_lock);
+ motor_lock.locked = 0;
+ motor_lock.thread = NULL;
#endif
=======================================
--- /trunk/src/lib/pid.c Mon Jan 17 21:17:15 2011
+++ /trunk/src/lib/pid.c Mon Jan 17 22:07:34 2011
@@ -59,8 +59,12 @@
float update_pid_input(struct pid_controller *pid, float current_val) {
+ printf ("current value %0.3f\n", current_val);
+
float error = pid->goal - current_val;
+ printf ("error %0.3f\n", error);
+
// proportional feedback
float result = error * pid->kp;
=======================================
--- /trunk/tests/panic_nonhalting.c Mon Jan 17 21:16:52 2011
+++ /trunk/tests/panic_nonhalting.c Mon Jan 17 22:07:34 2011
@@ -23,7 +23,7 @@
}
int umain(void) {
- create_thread(&panicbreak, 64, THREAD_PRIORITY_NORMAL, "panicbreak");
+ create_thread(&panicbreak, 64, 0, "panicbreak");
while (1) {
yield();
=======================================
--- /trunk/user/demo.c Mon Jan 17 21:16:52 2011
+++ /trunk/user/demo.c Mon Jan 17 22:07:34 2011
@@ -130,7 +130,7 @@
}
int umain(void) {
- create_thread(&display_angle, 64, THREAD_PRIORITY_NORMAL, "display
angle");
+ create_thread(&display_angle, 64, 0, "display angle");
struct pid_controller pid;
init_pid (&pid, 1, 0, 0, &read_angle, &set_turn);
=======================================
--- /trunk/user/mouse/mouse.c Mon Jan 17 21:16:52 2011
+++ /trunk/user/mouse/mouse.c Mon Jan 17 22:07:34 2011
@@ -375,7 +375,7 @@
uint8_t num_low_readings = 0;
activateRun();
- create_thread(&CaughtThread, STACK_DEFAULT,
THREAD_PRIORITY_NORMAL, "Caught Thread");
+ create_thread(&CaughtThread, STACK_DEFAULT, 0, "Caught Thread");
while(1){
=======================================
--- /trunk/user/umain2.c Mon Jan 17 21:16:52 2011
+++ /trunk/user/umain2.c Mon Jan 17 22:07:34 2011
@@ -81,7 +81,7 @@
}
int umain(void) {
- create_thread(&display_angle, 64, THREAD_PRIORITY_NORMAL, "display
angle");
+ create_thread(&display_angle, 64, 0, "display angle");
//struct pid_controller pid;
//init_pid (&pid, 1, 0, 0, &read_angle, &set_turn);