
//MEGAƬ
unit MEGA
{
	public const uint16 ID = 0;
	
	#.COM_MCU = RES;
	public RES =
	#include <system/cpu/avr/register_$chip$.txt>
	
	//---------------------------------------------------
	//[i] var_uint8 OS_time;
	public uint8 OS_time;
	
	//[i] var_uint8 OS_EventFlag;
	uint8 OS_EventFlag;
	//[i] event StartEvent;
	//[i] event IdleEvent;
	
	uint8 WatchDogTick;
	
	int8 M1power;
	int8 M1power_last;
	int8 M1A_DT;
	int8 M1B_DT;
	
	int8 M2power;
	int8 M2power_last;
	int8 M2A_DT;
	int8 M2B_DT;
	
	int8 M3power;
	int8 M3power_last;
	int8 M3A_DT;
	int8 M3B_DT;
	
	int8 M4power;
	int8 M4power_last;
	int8 M4A_DT;
	int8 M4B_DT;
	
	
	int8 tick;
	
	
	//[i] linkconst_int32_1 TT;
	public const int32 TT = 1;
	
	int8 T;
	int8 tt;
	
	//---------------------------------------------------
	//[i] function_void OS_init;
	public void OS_init()
	{
		FlashOpen = false;
		FlashForever = false;
		FlashNumber = 0;
		swap = 0;
		WatchDogTick = 0;
		OS_time = 50;
		OS_EventFlag = 0;
		OS_EventFlag.0(bit) = 1;
		
		//ڵȴ׼(ݸλ)
		loop( 10 ) {
			delay();
		}
		LED_DDR = 1;
		LED_PORT = 0;
		
		M1power = 100;
		M2power = 100;
		M3power = 100;
		M4power = 100;
		M1power_last = 100;
		M2power_last = 100;
		M3power_last = 100;
		M4power_last = 100;
		
		tick = 0;
		
		T = (int8)(int16)TT;
		tt = 0;
		
		PCA9685_init();
	}
	//---------------------------------------------------
	//[i] function_void OS_thread;
	public void OS_thread()
	{
		OS_EventFlag.1(bit) = 1;
	}
	//---------------------------------------------------
	//[i] function_void OS_run;
	public void OS_run()
	{
		//жϵͳ¼Ƿס, еλʾʱۼ, λ˸ʱ
		if( WatchDogTick >= 15 ) {
			WatchDogTick + 16;
			//LED_PORT = WatchDogTick.7(bit);
			return;
		}
		WatchDogTick + 1;
		
		//----------------------------------------------
		if( M1power_last != M1power ) {
			M1power_last = M1power;
			uint16 po = (uint)M1power;
			po *= 40;
			if( M1A_DT == 1 ) {
				PCA9685.write( 9, po );
			}
			if( M1B_DT == 1 ) {
				PCA9685.write( 10, po );
			}
		}
		if( M2power_last != M2power ) {
			M2power_last = M2power;
			uint16 po = (uint)M2power;
			po *= 40;
			if( M2A_DT == 1 ) {
				PCA9685.write( 11, po );
			}
			if( M2B_DT == 1 ) {
				PCA9685.write( 12, po );
			}
		}
		if( M3power_last != M3power ) {
			M3power_last = M3power;
			uint16 po = (uint)M3power;
			po *= 40;
			if( M3A_DT == 1 ) {
				PCA9685.write( 3, po );
			}
			if( M3B_DT == 1 ) {
				PCA9685.write( 4, po );
			}
		}
		if( M4power_last != M4power ) {
			M4power_last = M4power;
			uint16 po = (uint)M4power;
			po *= 40;
			if( M4A_DT == 1 ) {
				PCA9685.write( 5, po );
			}
			if( M4B_DT == 1 ) {
				PCA9685.write( 6, po );
			}
		}
		
		
		//˸
		if( !FlashOpen ) return;
		swap % 10;
		bit OUT;
		if( swap < 1 ) {
			OUT = 1;
		}
		else {
			OUT = 0;
		}
		swap + 1;
		
		if( FlashForever ) {
			LED_PORT = OUT;
			return;
		}
		if( FlashNumber == 0 ) {
			FlashOpen = false;
			return;
		}
		if( LED_PORT == 1 && OUT == 0 ) {
			FlashNumber - 1;
		}
		LED_PORT = OUT;
	}
	//---------------------------------------------------
	//[i] function_void OS_ClearWatchDog;
	public void OS_ClearWatchDog()
	{
		WatchDogTick = 0;
	}
	//---------------------------------------------------
	//[i] function_void set_servo0 int32;
	public void set_servo0( int32 d )
	{
		uint16 d1 = switch_a( d );
		PCA9685.write( 0, d1 );
	}
	//---------------------------------------------------
	//[i] function_void set_servo1 int32;
	public void set_servo1( int32 d )
	{
		uint16 d1 = switch_a( d );
		PCA9685.write( 1, d1 );
	}
	
	//---------------------------------------------------
	//[i] function_void set_servo14 int32;
	public void set_servo14( int32 d )
	{
		uint16 d1 = switch_a( d );
		PCA9685.write( 14, d1 );
	}
	
	//---------------------------------------------------
	//[i] function_void set_servo15 int32;
	public void set_servo15( int32 d )
	{
		uint16 d1 = switch_a( d );
		PCA9685.write( 15, d1 );
	}
	
	uint16 switch_a( int32 d )
	{
		if( d < -90 ) {
			d = -90;
		}
		if( d > 90 ) {
			d = 90;
		}
		uint16 d1 = (uint16)(uint)(d + 90);
		d1 = 102 + (d1 * 410 / 180);
		return d1;
	}
	//---------------------------------------------------
	//[i] function_void LightOpen;
	public void LightOpen()
	{
		FlashOpen = false;
		LED_PORT = 1;
	}
	//---------------------------------------------------
	//[i] function_void LightClose;
	public void LightClose()
	{
		FlashOpen = false;
		LED_PORT = 0;
	}
	//---------------------------------------------------
	//[i] function_void LightSwap;
	public void LightSwap()
	{
		FlashOpen = false;
		LED_PORT = ~LED_PORT;
	}
	//---------------------------------------------------
	//[i] function_void LightFlashTimes int32;
	public void LightFlashTimes( int32 n )
	{
		#asm "cli"
		FlashNumber = n;
		#asm "sei"
		swap = 0;
		FlashForever = false;
		FlashOpen = true;
	}
	//---------------------------------------------------
	//[i] function_void LightFlash;
	public void LightFlash()
	{
		#asm "cli"
		FlashNumber = 0;
		#asm "sei"
		FlashForever = true;
		FlashOpen = true;
	}
	
	//---------------------------------------------------
	//[i] function_void set_t int32;
	public void set_t( int32 t )
	{
		T = (int8)(int16)t;
		tt = 0;
	}
	
	//---------------------------------------------------
	void delay()
	{
		loop( 250 ) loop( 250 ) {}
	}
	
	uint8 swap;
	int32 FlashNumber;
	bool FlashOpen;
	bool FlashForever;
	link bit LED_DDR = RES.DDRB.5;
	link bit LED_PORT = RES.PORTB.5;
	
	//=================================================================================
	
	public link unit SDA {}
	public link unit SCL {}
	
	PCA9685.SDA = SDA;
	PCA9685.SCL = SCL;
	PCA9685 =
	#include <system\common\PCA9685.txt>
	
	//---------------------------------------------
	memory my_bit { type = [ uint8 0, 255 ] uint8; }
	
	public void PCA9685_init()
	{
		PCA9685.OS_init();
		PCA9685.write( 2, 4095 );
		PCA9685.write( 7, 4095 );
		PCA9685.write( 8, 4095 );
		PCA9685.write( 13, 4095 );
	}
	
	//---------------------------------------------
	void set_uint8( uint8 addr, uint8 b )
	{
		uint8 p = 0;
		if( addr == 3 || addr == 4 ) p = (uint)M3power;
		if( addr == 5 || addr == 6 ) p = (uint)M4power;
		if( addr == 9 || addr == 10 ) p = (uint)M1power;
		if( addr == 11 || addr == 12 ) p = (uint)M2power;
		
		uint16 po = p;
		po *= 40;
		
		if( b == 0 ) {
			PCA9685.write( addr, 0 );
		}
		else {
			PCA9685.write( addr, po );
		}
	}
}

#include "mio_$run$.txt"









