Index: trunk/code/projects/scheduler/scheduler.c
===================================================================
--- trunk/code/projects/scheduler/scheduler.c	(revision 1572)
+++ trunk/code/projects/scheduler/scheduler.c	(revision 1573)
@@ -3,28 +3,37 @@
  * @brief Scheduler
  *
  * Implementation of functions for scheduler
+ * Currently relies on avr-gcc's interrupt call convention.
  *
  * @author Colony Project, CMU Robotics Club
  * Based on avrOS and 18348 Lab9 code
  **/
 
+#include <avr/interrupt.h>
+#include <util/delay.h>
 #include "scheduler.h"
-#include "time.h"
 
-static uint8_t STACK[MAXTASKS][STACKSIZE];
+#define DEBUG
+
+#ifdef DEBUG
+#include <serial.h>
+#endif //DEBUG
+
+static uint8_t STACK[MAXTASKS + 1][STACKSIZE];
 static uint8_t nactive_tasks = 1;
 static uint8_t current_task = 0; //Default to main.
 
 //Internal functions 
-void task_terminate(void);
+static void task_terminate(void);
 
-//Scheduler gets called from the interrupt.
-void scheduler(void);
-void create_launch_stack(uint8_t task);
-void store_task(void);
-void restore_task(void); 
-void restore_next_task(void); 
+//TODO Use a context struct instead of pushing onto the stack.
+//so we don't need to worry so much about stack overflow.
+typedef struct context_t {
+	uint8_t reg[32];
+	uint8_t sreg;
+} context_t;
 
+//Define some PCB's
 typedef struct PCB_t
 {
 	void (*exec)(void);
@@ -36,9 +45,40 @@
 
 PCB_t PCB[MAXTASKS + 1];
 
-void scheduler_init() { }
+//Keep track of global time.
+static uint32_t current_time = 0;
 
-void task_terminate(void) { 
+unsigned int time_now() { 
+	return current_time;
+}
+
+void scheduler_init() { 
+	//Set the counter to 0.
+	TCNT3 = 0;
+	
+	//Clear timer on compare (CTC) mode.
+	TCCR3B |= _BV(WGM32);
+
+	//Set a prescalar of 8. 8 / (8MHz) = 1us per tick.
+	TCCR3B |= _BV(CS31);
+
+	//Trigger an interrupt every 16th second for now.
+	OCR3A = 0xF424;
+
+	//Enable Output Compare A Interrupt. (Begins immediately).
+  	ETIMSK |= _BV(OCIE3A);
+
+#ifdef DEBUG
+	usb_init();
+#endif //DEBUG
+	
+	sei();
+}
+
+static void task_terminate(void) { 
+#ifdef DEBUG
+	usb_puts("In task terminate!.\n\r");
+#endif //DEBUG
 	PCB[current_task].running = 0;
 	yield();
 }
@@ -56,7 +96,7 @@
 	
 	PCB[nactive_tasks].exec = exec;
 	PCB[nactive_tasks].period = period;
-	PCB[nactive_tasks].next = rtc_get() + period;
+	PCB[nactive_tasks].next = current_time + period;
 	PCB[nactive_tasks].running = 0;	
 	
 	//Don't need to initialize SP, it will get done later.
@@ -64,186 +104,119 @@
 	return nactive_tasks - 1;
 }
 
-//Create a fresh launch stack. 
-void create_launch_stack(uint8_t task) {
-	uint8_t* sp = &STACK[task][STACKSIZE - 1];
+//I need a timer to do the rest.  Thinking about stealing it from rtc.
+SIGNAL(TIMER3_COMPA_vect) {
+	static uint8_t task_i = 1;
+	volatile uint8_t** sploc;
+	volatile uint8_t* sp;
 	
-	//Put task terminate  and the task to execute on the stack.
-	*(sp--) = (uint8_t)(uint16_t) *task_terminate;
-	*(sp--) = (uint8_t)(uint16_t) *task_terminate >> 8;
-	*(sp--) = (uint8_t)(uint16_t) *PCB[nactive_tasks].exec;
-	*(sp--) = (uint8_t)(uint16_t) *PCB[nactive_tasks].exec >> 8;
+	uint8_t i;
 	
-	//This is going to get me in trouble,
-	//but store_task already does everything else we need
-	//(since we only care about void-void functions)
-	store_task();	
-}
+#ifdef DEBUG
+	char test_buf[128];
+#endif //DEBUG
+	
+	//Store sp.
+	sploc = &(PCB[current_task].sp);
+	
+	asm volatile( \
+		"in r0, __SP_L__ \n" \
+	   	"st %a0+, r0 \n" \
+	    "in r0, __SP_H__ \n" \
+		"st %a0, r0 \n"  : : "e" (sploc));
+	
+#ifdef DEBUG
+	sprintf(test_buf, "current_task = %d, current_time = %d, sp = %x\n\r", current_task, current_time, sploc);
+	usb_puts(test_buf);
+#endif //DEBUG
+	
+	//The current task may no longer be running when we exit.
+	PCB[current_task].running = 0;
+	
+	//Keep track of the number of 1/16 second's that have passed.
+	current_time++;
+	
+	/******** scheduler ********/
 
-void scheduler(void){
-	static uint8_t task_i = 1;
-	uint8_t i;
-	int current_time = rtc_get();
-	
 	//Loop over registered tasks, like in round robin order.
-	for(i = nactive_tasks; i > 0; i--) { 
-		if(		!PCB[task_i].running
-			&&   PCB[task_i].next <= current_time) { 
+	for(i = nactive_tasks - 1; i > 0; i--) { 
+		
+		//Launch a new task if it is due to run.
+		if(	!PCB[task_i].running
+		  && PCB[task_i].next <= current_time) { 
 			
+			task_i++;
+			
 			current_task = task_i;	
 			PCB[task_i].next += PCB[task_i].period;
-			create_launch_stack(task_i);
-			task_i++;
+		
+			/***** create launch stack ***/
+#ifdef DEBUG
+			usb_puts("Switching tasks..... (cross fingers)\n\r");
+#endif //DEBUG
+
+			sp = &(STACK[task_i][STACKSIZE - 1]);
+			
+			//Put task terminate  and the task to execute on the stack.
+			*(sp--) = (uint8_t)(uint16_t) task_terminate;
+			*(sp--) = (uint8_t)(uint16_t) task_terminate >> 8;
+			*(sp--) = (uint8_t)(uint16_t) PCB[nactive_tasks].exec;
+			*(sp--) = (uint8_t)(uint16_t) PCB[nactive_tasks].exec >> 8;
+			
+			//The current state of the registers is what the tasks will
+			//see on entry (shouldn't matter).
+
+			asm volatile( \
+				"out __SP_L__, %A0 \n" \
+				"out __SP_H__, %B0 \n" : : "d" (sp));
+
+#ifdef DEBUG
+			sprintf(test_buf, "Switching tasks,  sp = %x\n\r", sp);
+			usb_puts(test_buf);
+#endif //DEBUG
+
+	 		/* start process and enable interrupts */
+	 		asm volatile("reti \n");
 			break;
 		}
+		
+		//Continue an old task.
+		if(PCB[task_i].running) {
+			current_task = task_i;
+			
+			//Reset the stack pointer.
+			sp = PCB[current_task].sp;
+			asm volatile( \
+				"out __SP_L__, %A0 \n" \
+				"out __SP_H__, %B0 \n" : : "d" (sp));
 
-		if( 	PCB[task_i].running ) {
-			current_task = task_i;
 			task_i++;
 			break;
 		}
 		
-		//Loop back to 0 if necessary.
+		//Loop back to 1 if necessary.
 		task_i++;
-		if(task_i >= nactive_tasks) 
+		if(task_i >= nactive_tasks)  
 			task_i = 1;
 	}
 	
-	//If no task was selected to run,
+	//If no task was selected to run, go to main
 	if(i == 0) { 
-		//Return to main.
+#ifdef DEBUG
+		usb_puts("Resetting main.\n\r");
+#endif //DEBUG
+		//Set the SP to the original stack.
+		sp = PCB[current_task].sp;
+		asm volatile( \
+			"out __SP_L__, %A0 \n" \
+			"out __SP_H__, %B0 \n" : : "d" (sp));
+		
 		current_task = 0;	
 	}
-}
-
-void store_task(void) { 
-	//Store all state for this task.
+		
+	//Set the running task.*/
+	PCB[current_task].running = 1;
 	
-	/* store general purpose registers */
-	asm volatile( \
-	 	"push r31 \n" \
-	   	"push r30 \n" \
-	    "push r29 \n" \
-		"push r28 \n" \
-		"push r27 \n" \
-		"push r26 \n" \
-		"push r25 \n" \
-		"push r24 \n" \
-		"push r23 \n" \
-		"push r22 \n" \
-		"push r21 \n" \
-		"push r20 \n" \
-		"push r19 \n" \
-		"push r18 \n" \
-		"push r17 \n" \
-		"push r16 \n" \
-		"push r15 \n" \
-		"push r14 \n" \
-		"push r13 \n" \
-		"push r12 \n" \
-		"push r11 \n" \
-		"push r10 \n" \
-		"push r9 \n" \
-		"push r8 \n" \
-		"push r7 \n" \
-		"push r6 \n" \
-		"push r5 \n" \
-		"push r4 \n" \
-		"push r3 \n" \
-		"push r2 \n" \
-		"push r1 \n" \
-		"push r0 \n");
-	 
-	 //Store status register.
-	asm volatile( \
-	  	"in r0, __SREG__ \n" \
-	   	"push r0 \n");
-
-	//Store sp.
-	uint16_t sploc = (uint16_t)&(PCB[current_task].sp);
-	asm volatile( \
-		"in r0, __SP_L__ \n" \
-	   	"st Z+, r0 \n" \
-	    "in r0, __SP_H__ \n" \
-		"st Z, r0 \n"  : : "e" (sploc));
-
+	//avr-gcc inserts all of the necessary pops and the reti here.
 }
 
-//Figure out which task to run next, then run it.
-void restore_task(void) { 
-	
-	//Again, we never return.
-	asm volatile( \
-	 	"pop r0 \n" \
-	 	"pop r0 \n");
-
-	/* restore stack pointer */
-	uint16_t sp = (uint16_t)PCB[current_task].sp;
-	asm volatile( \
-		"out __SP_L__, %A0 \n" \
-		"out __SP_H__, %B0 \n" : : "d" (sp));
-
-	/* restore status register */
-	 asm volatile( \
-	  	"pop r0 \n" \
-	   	"out __SREG__, r0 \n");
-
-	 /* restore general purpose registers */
-	 asm volatile( \
-	 	"pop r0 \n" \
-	 	"pop r1 \n" \
-		"pop r2 \n" \
-		"pop r3 \n" \
-		"pop r4 \n" \
-		"pop r5 \n" \
-		"pop r6 \n" \
-		"pop r7 \n" \
-		"pop r8 \n" \
-		"pop r9 \n" \
-		"pop r10 \n" \
-		"pop r11 \n" \
-		"pop r12 \n" \
-		"pop r13 \n" \
-		"pop r14 \n" \
-		"pop r15 \n" \
-		"pop r16 \n" \
-		"pop r17 \n" \
-		"pop r18 \n" \
-		"pop r19 \n" \
-		"pop r20 \n" \
-		"pop r21 \n" \
-		"pop r22 \n" \
-		"pop r23 \n" \
-		"pop r24 \n" \
-		"pop r25 \n" \
-		"pop r26 \n" \
-		"pop r27 \n" \
-		"pop r28 \n" \
-		"pop r29 \n" \
-		"pop r30 \n" \
-		"pop r31 \n");
-
-	 /* start process and enable interrupts */
-	 //Note that either create_launch_stack or a timer interrupt
-	 //already has stored the value to load into PC
-	 asm volatile("reti \n");
-}
-
-void restore_next_task(void) { 
-	//Again, we never return.
-	asm volatile( \
-	 	"pop r0 \n" \
-	 	"pop r0 \n");
-	
-	PCB[current_task].running = 0;
-	scheduler();
-	PCB[current_task].running = 1;
-	restore_task();
-}
-
-//I need a timer to do the rest.  Thinking about stealing it from rtc.
-//ISR needs to 
-//a) store_task
-//b) restore_next_task.
-
-
Index: trunk/code/projects/scheduler/scheduler.h
===================================================================
--- trunk/code/projects/scheduler/scheduler.h	(revision 1572)
+++ trunk/code/projects/scheduler/scheduler.h	(revision 1573)
@@ -12,15 +12,16 @@
 #ifndef _SCHEDULER_H_
 #define _SCHEDULER_H_
 
-#define STACKSIZE 64
-#define MAXTASKS 16
+#define STACKSIZE 128
+#define MAXTASKS 3
 
-#define PRIORITY_LOWEST 8
-
 #include <stdint.h>
 
 void scheduler_init(void);
 void yield(void);
+
 int register_task(void (*exec)(void), uint16_t period);
 
+unsigned int time_now(void);
+
 #endif
Index: trunk/code/projects/scheduler/main.c
===================================================================
--- trunk/code/projects/scheduler/main.c	(revision 1572)
+++ trunk/code/projects/scheduler/main.c	(revision 1573)
@@ -1,26 +1,47 @@
-#include <dragonfly_lib.h>
+#include <serial.h>
+#include <avr/interrupt.h>
 #include "scheduler.h"
 
+void task0(void);
+void task1(void);
+void task2(void);
+
 int main(void)
 {
+	//Granularity is currently seconds, 
+	usb_init();
 
-	/* initialize components, set wireless channel */
-	dragonfly_init(ALL_ON);
-	
-	int val;
+	usb_puts("Oh no!!! Reset!!\n\r");
 
+	scheduler_init();
+	register_task(task0, 22);
+	//register_task(task1, 2);
+	//register_task(task2, 3);
+
 	while (1) {
-
-		usb_puts("Val: ");
-		bom_refresh(BOM_ALL);
-		usb_puti(bom_get_max());
-
-		usb_putc('\n');
-
+		//usb_puti(time_now());
+		usb_puts("main\n\r");
 		delay_ms(200);
-
 	}
 
 	return 0;
 }
 
+void task0() { 
+	sei();
+	usb_puts("0\n\r");
+	
+	cli();
+}
+
+void task1() { 
+	sei();
+	usb_puts("1\n\r");
+	cli();
+}
+
+void task2() { 
+	sei();
+	usb_puts("2\n\r");
+	cli();
+}
