// return motor status
void Get_Drive_Status(void)
{
short theData;
putstr("DTarget: ");
putS16(drive.targetSpeed);
putstr("\r\n Dcurrent:");
putS16(encoder1_speed);
putstr("\r\n curPWM ");
putS16(drive.currentPWM);
putstr("\r\n odo ");
putS16((unsigned short)odometer);
putstr("\r\nIsense:");
theData = A2D_read_channel(CURRENT_SENSE_CHANNEL);
putS16(theData);
putstr("\r\n");
}
void doFade(illuminatorStruct *illum){
int diff;
putstr("doFade rcount");
illum->rInc = 0;
illum->gInc = 0;
illum->bInc = 0;
if(illum->Time == 0) {
illum->R = illum->tR;
illum->G = illum->tG;
illum->B = illum->tB;
}
/* to fade, find difference (amount we need to change) */
/* delay count will be inversely proportional to diff */
/* i.e. more increments per time for bigger difference */
/* delay count will be proportional to time param */
/* i.e. wait longer for longer time */
else {
diff = illum->tR - illum->R;
if (diff > 0)
illum->rInc = 1;
else if (diff < 0) {
illum->rInc = -1;
diff = -diff;
}
illum->rCount = diff==0? 0 : (int) (256/diff);
illum->rCount *= illum->Time;
putS16(illum->rCount );
diff = illum->tG - illum->G;
if (diff > 0)
illum->gInc = 1;
else if (diff < 0) {
illum->gInc = -1;
diff = -diff;
}
illum->gCount = diff==0? 0 : (int) (256/diff);
illum->gCount *= illum->Time;
diff = illum->tB - illum->B;
if (diff > 0)
illum->bInc = 1;
else if (diff < 0) {
illum->bInc = -1;
diff = -diff;
}
illum->bCount = diff==0? 0 : (int) (256/diff);
illum->bCount *= illum->Time;
}
}
void TestSerialization::testVecPut()
{
std::vector<u8> buf;
putU8(&buf, 0x11);
putU16(&buf, 0x2233);
putU32(&buf, 0x44556677);
putU64(&buf, 0x8899AABBCCDDEEFF);
putS8(&buf, -128);
putS16(&buf, 30000);
putS32(&buf, -6);
putS64(&buf, -43);
putF1000(&buf, 53.53467f);
putF1000(&buf, -300000.32f);
putF1000(&buf, F1000_MIN);
putF1000(&buf, F1000_MAX);
putString(&buf, "foobar!");
putV2S16(&buf, v2s16(500, 500));
putV3S16(&buf, v3s16(4207, 604, -30));
putV2S32(&buf, v2s32(1920, 1080));
putV3S32(&buf, v3s32(-400, 6400054, 290549855));
putV2F1000(&buf, v2f(500.65661f, 350.34567f));
putWideString(&buf, L"\x02~woof~\x5455");
putV3F1000(&buf, v3f(500, 10024.2f, -192.54f));
putARGB8(&buf, video::SColor(255, 128, 50, 128));
putLongString(&buf, "some longer string here");
putU16(&buf, 0xF00D);
UASSERT(buf.size() == sizeof(test_serialized_data));
UASSERT(!memcmp(&buf[0], test_serialized_data, sizeof(test_serialized_data)));
}
void Drive_Servo_Task(void)
{
short speedError;
short drivePWM;
int16_t p_term, d_term, i_term;
// close enough to motionless
if(abs(drive.targetSpeed) <= drive.dead_band){
drive.targetSpeed = 0;
Set_Motor1_PWM(0, FORWARD);
return;
}
// get current speed
drive.currentSpeed = encoder1_speed;
// calculate error term
speedError = drive.targetSpeed - drive.currentSpeed;
// if currentSpeed is less than target, speed up: positive PWM
// if currentSpeed is more than target, slow down: negative PWM
if (abs(speedError) < drive.dead_band) {
// we are close enough to desired speed
// don't change anything
if (Drive_Debug_Output == 1)
putstr("DRIVE speed OK ");
return;
}
//if here, error is significant; change PWM to decrease it
// calculate proportional term
p_term = speedError * drive.Kp;
// calculate derivative term
d_term = drive.Kd * (drive.lastSpeedError - speedError);
drive.lastSpeedError = speedError;
// sum to integrate steering error
iSum += speedError;
// and limit runaway
limit(&iSum,-drive.intLimit,drive.intLimit);
i_term = drive.Ki*iSum;
i_term = i_term /8; // (divide by 4) to scale
drivePWM = p_term + d_term + i_term;
drivePWM = drivePWM / 32; // (divide to scale)
drive.currentPWM = drivePWM;
// If Debug Log is turned on, output PID data until position is stable
if (Drive_Debug_Output == 1) {
putstr("DRIVEspd targ curr ");
putS16(drive.targetSpeed);
putS16(drive.currentSpeed);
putstr(" DrivePWM: ");
putS16(drivePWM);
putstr(" P,I,D: ");
putS16(p_term);
putS16(i_term);
putS16(d_term);
putstr(" int ");
putS16(iSum);
putstr("\r\n");
}
// use computed PWM to drive the motor
// check current limit here -- reduce power if so?
if (drivePWM < 0) {
drive.currentDirection = REVERSE;
drivePWM = abs(drivePWM);
}
else
drive.currentDirection = FORWARD;
if(drivePWM < drive.minPWM){
drivePWM = drive.minPWM;
if (Drive_Debug_Output)
putstr("DRIVE under min\n");
}
// take care of absolute min and max
// make sure arg2 < arg3
limit( &drivePWM, drive.minPWM, drive.maxPWM);
// limit change of drive: make sure arg2 < arg3
limit( &drivePWM, drive.lastPWM - drive.deltaAccel,
drive.lastPWM + drive.deltaAccel);
Set_Motor1_PWM((unsigned char) drivePWM, drive.currentDirection );
//if(drive.currentDirection == FORWARD)
// Set_Motor1_PWM( drivePWM, drive.currentDirection );
//else
// Set_Motor1_PWM(0, FORWARD );
drive.lastPWM = (unsigned char)drivePWM;
}
void Motor_dump_data(void)
{
short theData;
putstr("Drive: (targ cur PWM) ");
putS16(drive.targetSpeed);
putS16(encoder1_speed);
putS16(drive.currentPWM);
putstr(" odo ");
putS16((unsigned short)odometer);
putstr(" PID:");
putS16(drive.Kp);
putS16(drive.Ki);
putS16(drive.Kd);
putstr(" ISense: ");
theData = A2D_read_channel(CURRENT_SENSE_CHANNEL);
putS16(theData);
// putstr("\r\n");
putstr("\n dead minP maxP delA");
putS16(drive.dead_band);
putS16(drive.minPWM);
putS16(drive.maxPWM);
putS16(drive.deltaAccel);
putstr("\r\n");
}
int main( void ){
unsigned char cData; /* byte to read from UART */
unsigned int rdelay=0;
unsigned int gdelay=0;
unsigned int bdelay=0;
/* set up DDRD for serial IO */
DDRD = 0xF2; /* 0b 1111 0010 1 indicates Output pin */
// led_port = 0xF0; // turn OFF leds
UART_Init(11); // 12 = 38.4k when system clock is 8Mhz (AT Tiny2313)
//11 = 38.4K for 7.37MHz xtal
// 51 = 9600 baud when system clock is 8Mhz
sei();
DDRB |= _BV(PB0); /* PB0 is spare debug */
DDRB |= _BV(PB1); /* PB1 is RED output */
DDRB |= _BV(PB2); /* PB2 is GRN output */
DDRB |= _BV(PB3); /* PB3 is BLU output */
putstr("\r\n...Illuminator at address ");
illum.Addr = readAddressEEPROM();
putS16((short)illum.Addr );
putstr(" says hello...\r\n");
/* init data structure */
illum.H=0;
illum.S=0;
illum.V=0;
illum.R=0; /* raw RGB output vales */
illum.G=0;
illum.B=0;
illum.tR=0; /* target RGB values */
illum.tG=0;
illum.tB=0;
illum.rCount=0; /* fade delay counts */
illum.gCount=0;
illum.bCount=0;
illum.rInc =0; /* fade increment counts */
illum.gInc =0; /* fade increment counts */
illum.bInc =0; /* fade increment counts */
illum.Time=0;
illum.Now=0;
illum.fading=0;
// =======================================================
while(1) {
// Main parser loop starts here. To save space, not modularized
if (UART_data_in_ring_buf()) { // check for waiting UART data from SPU
cData = UART_ring_buf_byte(); // get next char from ring buffer...
if(accumulateCommandString(cData)){ // ... and add to command string
// if we are here we have a complete command; parse it
parseCommand(); // parse and execute commands
}
}
#ifdef INVERT
/* main PWM loop is free-running here INVERTED VERSION */
PORTB=0x00; /* turn on all LEDs */
for(pwm=0;pwm<255;pwm++){
if(pwm >= gtab[illum.R]) /* if we've reached red value turn off R bit */
PORTB |= _BV(PB1);
if(pwm >= gtab[illum.G])
PORTB |= _BV(PB2); /* if we've reached grn value turn off B bit */
if(pwm >= gtab[illum.B])
PORTB |= _BV(PB3); /* if we've reached blu value turn off G bit */
#else
/* main PWM loop is free-running here */
PORTB=0x0F; /* turn on all LEDs */
for(pwm=0;pwm<255;pwm++){
if(pwm >= gtab[illum.R]) /* if we've reached red value turn off R bit */
PORTB &=~_BV(PB1);
if(pwm >= gtab[illum.G])
PORTB &=~_BV(PB2); /* if we've reached grn value turn off B bit */
if(pwm >= gtab[illum.B])
PORTB &=~_BV(PB3); /* if we've reached blu value turn off G bit */
#endif
/* IF we are fading, AND we're not there, AND we've waited enough... */
if (illum.rInc && (illum.R != illum.tR) && (rdelay++ >= illum.rCount)) {
rdelay = 0;
illum.R += illum.rInc; /* increment towards target */
if (illum.R == illum.tR){
illum.rInc = 0; /* we've reached our target, stop increment */
}
}
if (illum.gInc && (illum.G != illum.tG) && (gdelay++ >= illum.gCount)) {
gdelay = 0;
illum.G += illum.gInc; /* increment towards target */
if (illum.G == illum.tG) {
illum.gInc = 0; /* we've reached our target, stop increment */
}
}
if (illum.bInc && (illum.B != illum.tB) && (bdelay++ >= illum.bCount)) {
bdelay = 0;
illum.B += illum.bInc; /* increment towards target */
if (illum.B == illum.tB)
illum.bInc = 0; /* we've reached our target, stop increment */
}
}
}
}
