// filename  FuzMotor.C

// simple fuzzy logic motor controller  

// Last modified 1/16/02 by Jonathan W. Valvano

 

// Copyright 2002 by Jonathan W. Valvano, valvano@uts.cc.utexas.edu 

//    You may use, edit, run or distribute this file 

//    as long as the above copyright notice remains 



#include "HC12.h"

short SpeedError;    // 0 to 255, 128 means no error

short Acceleration;  // 0 to 255, 128 means no acceleration

short U;             // 0 to 255, 0 means no power

short dU;            // -128 to +127, 0 means no power change

short Xstar;         // desired speed, 0 to 255

short Xlast;         // previous speed, 0 to 255

short Xprime;        // estimated speed, 0 to 255

short Demote(short);

unsigned char Sensor(unsigned char);

asm(".include 'fuzzy.asm' ");

void main(void){ 

  TSCR = 0x80;

  ATDCTL2 = 0x80;  // ADPU=1 enables A/D

  DDRH = 0xFF;     // PortH are output pins

  U = 0;           // U is actuator control

  Xstar = 128;     // desired speed

  Xlast = 0;       // previous speed



  while(1){

    Xprime = Sensor(2);  // estimated speed

    SpeedError = Xprime-Xstar;

    SpeedError = Demote(SpeedError); // scale to 0 to 255 

    Acceleration = Xprime-Xlast;

    Acceleration = Demote(Acceleration);

    Xlast = Xprime;  // setup for next

    dU = FuzzyLogic(SpeedError,Acceleration);

    U = U+dU;

    if(U<0){ 

      U = 0;

    }

    if(U>255){

      U = 255;    // 0<=U<=255

    }

    PORTH=U;    // set DAC actuator 

  }

}



//*******Demote***********

// Input signed 16 bit

// add 128 to make unsigned

// check for bounds so output is 0 to 255

short Demote(short data){ signed x;

  x=data+128;

  if(x>255) 

    x = 255; // make sure it is within bounds

  if(x<0)   

    x = 0;   // 0 <= x <= +255

  return x;

}



//*******Sensor**************

// Input: ADC channel

// Function: Sample ADC

// Output: ADC result 0 to 255

unsigned char Sensor(unsigned char channel){

  ATDCTL5 =  channel;         // start conversions on specified channel

  while((ATDSTAT&0x01)==0){}; // wait for CCF0

  return ADR0H;  

}



extern void _start();	

#pragma abs_address:0xfffe

void (*reset_vector[])() = { _start };

#pragma end_abs_address