/*

avr-gcc -mmcu=atmega8 -O -g -Wall -o ballbeamctrl_a ballbeamctrl.c   
avr-objcopy -Osrec ballbeamctrl_a ballbeamctrl_a.sr
uisp -dprog=stk200 --erase --upload if=ballbeamctrl_a.sr
avr-objdump -S ballbeamctrl_a

> simcom -115200 /dev/ttyS0

*/

#include "trtkernel.c"

/********************* BALL AND BEAM APPLICATION ****************************/

// Controller parameters (fixed-point with 11 fractional bits)
#define ad 1418
#define bd -3547
#define ai -3
#define K_outer -512
#define K_inner 2

// Global variables used for inter-task communication
// (protected by semaphores)
int u1 = 0;
int u2 = 0;

int angleRef1 = 0;
int angleRef2 = 0;

int ref = 0;

// Data for PWM tasks
struct PWMdata {
  uint32_t t;
  uint8_t channel;
  uint8_t sem;
  int *uPtr;
};

// Data for inner controller tasks
struct ICdata {
  uint32_t t;
  uint32_t d;
  uint8_t ang_channel;
  uint8_t low;
  uint8_t high;
  uint16_t value;
  int angle;
  uint8_t sem1;
  uint8_t sem2;
  int u;
  int *angleRefPtr;
  int *uPtr;
};

// Data for outer controller tasks
struct OCdata {
  uint32_t t;
  uint32_t d;
  uint8_t pos_channel;
  uint8_t low;
  uint8_t high;
  uint8_t value;
  int pos;
  uint8_t sem;
  int *refPtr;
  int *angleRefPtr;
  int P;
  int I;
  int D;
  int yold;
};

/**
 * Interrupt handler for receiving characters 
 * on the serial connection
 */
SIGNAL(SIG_UART_RECV){
  char ch = inp(UDR);
  if (ch == 32) {
    ref = -ref;
  }
  if (ch == 's') {
    ref = 6554; /* 4 V */
  }
}

/**
 * Interrupt handler for data transmission,
 * called when send buffer empty to 
 * repeatedly send info on running task.
 */
SIGNAL(SIG_UART_DATA){
  outp('0'+kernel.running, UDR);
}


/**
 * Generic code function for the PWM tasks
 */
void PWMtask(void* args) {

  struct PWMdata* d = (struct PWMdata*) args;

  d->t = trtGetRelease();
  while (1) {
    PORTB |= d->channel; // AnalogOut +10 volt

    trtWait(d->sem); // Semaphore protecting u (global)
    d->t += ((uint16_t)(*(d->uPtr) + 512)) >> 3;
    trtSignal(d->sem);

    trtSleepUntil(d->t, 0);
    
    PORTB &= ~(d->channel); // AnalogOut -10 volt

    trtWait(d->sem); // Semaphore protecting u (global)
    d->t += 128 - (((uint16_t) (*(d->uPtr) + 512)) >> 3);
    trtSignal(d->sem);

    trtSleepUntil(d->t, 0);
  }
}

/**
 * Generic code function for the inner controller tasks
 */
void InnerCtrl(void* args) {

  struct ICdata* d = (struct ICdata*) args;
  
  d->t = trtGetRelease();
  d->d = trtGetDeadline();
  while(1) {
    // AnalogIn angle
    ADMUX = d->ang_channel;

    // Read angle sensor  
    ADCSRA |= 0x40;         // start conversion
    while (ADCSRA & 0x40);  // wait for conversion to finish

    d->low  = ADCL;             // read value
    d->high = ADCH;
    d->value = (d->high<<8) | (d->low);
    d->angle = (d->value - 512) << 5; 

    trtWait(d->sem1); // Semaphore protecting angleRef (global)
    d->u = (-K_inner*(*(d->angleRefPtr) - d->angle)) >> 5;
    trtSignal(d->sem1);

    trtWait(d->sem2); // Semaphore protecting u (global)
    *(d->uPtr) = d->u;
    // limit control
    if (d->u > 511) {
      *(d->uPtr) = 511;
    } else if (d->u < -512) {
      *(d->uPtr) = -512;
    }
    trtSignal(d->sem2);

    // Sleep
    d->t += SECONDS2TICKS(0.02);
    d->d += SECONDS2TICKS(0.02);
    trtSleepUntil(d->t, d->d);
  }
}

/**
 * Generic code function for the outer controller tasks
 */
void OuterCtrl(void* args) {

  struct OCdata* d = (struct OCdata*) args;
  
  d->t = trtGetRelease();
  d->d = trtGetDeadline();
  while(1) {
    // AnalogIn position
    ADMUX = d->pos_channel;
    
    // Read position sensor  
    ADCSRA |= 0x40;         // start conversion
    while (ADCSRA & 0x40);  // wait for conversion to finish

    d->low = ADCL;             // read value
    d->high = ADCH;
    d->value = (d->high<<8) | (d->low);
    d->pos = (d->value - 512) << 5; 

    // Calculate Output
    d->D = ((long)ad*(long)(d->D) - (long)bd*(long)(d->pos - d->yold)) >> 11;
    d->P = ((long)K_outer*(long)(*(d->refPtr)-d->pos)) >> 11;

    trtWait(d->sem); // Semaphore protecting angleRef (global)
    (*d->angleRefPtr) = d->P + d->I + d->D; 
    trtSignal(d->sem);

    d->I = d->I + (int)(((long)ai*(long)(*(d->refPtr) - d->pos)) >> 11);
    d->yold = d->pos;

    // Sleep
    d->t += SECONDS2TICKS(0.04);
    d->d += SECONDS2TICKS(0.04);
    trtSleepUntil(d->t, d->d);
  }
}

/* Data structures for the individual tasks */
struct PWMdata pd[2];
struct ICdata  id[2];
struct OCdata  od[2];

/**
 * Main Program.
 */
int main(void) {

  uint8_t i;
  
  DDRB = 0xff;    // Bit 0,1,3-5 output
  
  DDRC = 0x30;    // Bit 4-5 output
  ADCSRA = 0xc7;  // ADC enable (Rolf's error/fault/failure)

  /* Configure serial communication */
  outp(0x00, UCSRA);   // USART: 
  outp(0xa8, UCSRB);   // USART: RXC enable, UDRE enable, Receiver enable, Transmitter enable
  // outp(0xa8, UCSRB);   // UDRE disable, no monitoring  
  outp(0x86, UCSRC);   // USART: 8bit, no parity
  outp(0x00, UBRRH);   // USART: 38400 @ 14.7456MHz
  outp(7,    UBRRL);   // USART: 38400 @ 14.7456MHz, table p. 157

  // Initialize task data
  
  pd[0].channel = 0x02; // analogout channel 1
  pd[0].sem = 1;        // semaphore protecting u1
  pd[0].uPtr = &u1;     // pointer to global variable
  
  pd[1].channel = 0x04; // analogout channel 2
  pd[1].sem = 2;        // semaphore protecting u2
  pd[1].uPtr = &u2;     // pointer to global variable

  id[0].ang_channel = 0xc2; // ADMUX value for analogin channel 
  id[0].sem1 = 3;       // semaphore protecting angleRef1
  id[0].sem2 = 1;       // semaphore protecting u1
  id[0].uPtr = &u1;     // pointer to global variable
  id[0].angleRefPtr = &angleRef1;  // pointer to global variable
  
  id[1].ang_channel = 0xc0; // ADMUX value for analogin channel 
  id[1].sem1 = 4;       // semaphore protecting angleRef2
  id[1].sem2 = 2;       // semaphore protecting u2
  id[1].uPtr = &u2;     // pointer to global variable
  id[1].angleRefPtr = &angleRef2;  // pointer to global variable

  od[0].pos_channel = 0xc3; // ADMUX value for analogin channel 
  od[0].sem = 3;        // semaphore protecting angleRef1
  od[0].refPtr = &ref;  // pointer to global variable
  od[0].angleRefPtr = &angleRef1;  // pointer to global variable
  od[0].I = 0;
  od[0].D = 0;
  od[0].yold = 0;

  od[1].pos_channel = 0xc1; // ADMUX value for analogin channel 
  od[1].sem = 4;        // semaphore protecting angleRef2
  od[1].refPtr = &ref;  // pointer to global variable
  od[1].angleRefPtr = &angleRef2;  // pointer to global variable
  od[1].I = 0;
  od[1].D = 0;
  od[1].yold = 0;

  trtInitKernel(80);

  // Create Semaphores (must be done before creating the tasks that use them)
  for (i=1; i<=4; i++) {
    trtCreateSemaphore(i, 1);
  }

  trtCreateTask(PWMtask, 100, SECONDS2TICKS(0.0), SECONDS2TICKS(0), &(pd[0]));
  trtCreateTask(PWMtask, 100, SECONDS2TICKS(0.0), SECONDS2TICKS(0), &(pd[1]));

  trtCreateTask(InnerCtrl, 100, SECONDS2TICKS(0.015), SECONDS2TICKS(0.02), &(id[0]));
  trtCreateTask(InnerCtrl, 100, SECONDS2TICKS(0.015), SECONDS2TICKS(0.02), &(id[1]));

  trtCreateTask(OuterCtrl, 150, SECONDS2TICKS(0.005), SECONDS2TICKS(0.04), &(od[0]));
  trtCreateTask(OuterCtrl, 150, SECONDS2TICKS(0.005), SECONDS2TICKS(0.04), &(od[1]));

  // Idle task
  while (1);

}