int main()
{
setup_led_pins();
// current color
volatile rgb_t pwm;
// next color
volatile rgb_t color;
// enable random number generator
RNG_SHORTS = RNG_SHORTCUT_VALRDY_STOP;
RNG_START = 1;
while(true)
{
// fade towards color
while (color.red != pwm.red || color.green != pwm.green || color.blue != pwm.blue)
{
PWM(&pwm);
// for (uint8_t i=0; i<1; i++)
// {
if (color.red > pwm.red)
pwm.red++;
if (color.red < pwm.red)
pwm.red--;
if (color.green > pwm.green)
pwm.green++;
if (color.green < pwm.green)
pwm.green--;
if (color.blue > pwm.blue)
pwm.blue++;
if (color.blue < pwm.blue)
pwm.blue--;
// }
}
// display this color for a while
for (uint8_t i=0; i<100; i++)
PWM(&color);
// choose new, random color
color.red = random();
color.green = random();
color.blue = random();
}
return 0;
}
void Motor_Init(void)
{
MotorParams[0].dead_zone = PWM(66);
MotorParams[0].dead_zone_move = PWM(48);
MotorParams[1].dead_zone = PWM(66);
MotorParams[1].dead_zone_move = PWM(48);
MotorParams[0].acccumulated_distance = 0;
MotorParams[0].accumulated_distance_remainder = 0;
MotorParams[1].acccumulated_distance = 0;
MotorParams[1].accumulated_distance_remainder = 0;
}
int Servo_Position(int Degrees){
/*Local Variables*/
int Position = 0;
/*Degrees to PWM Pulse duration*/
Position = (Degrees * 10) + 500;
PWM(TIM22,Position,GPIOC,6);
return(Degrees);
}
/* Auto-increment set multiple channels at once with an array of 12bit values
*/
void PCA9685::PWM8(uint8_t start, uint8_t numch, uint8_t* values)
{
uint16_t newvals [numch];
memset(newvals,0,numch*2);
for(int ch = 0; ch < numch; ch++) {
newvals[ch] = CIELPWM(values[ch]);
}
PWM(start, numch, newvals);
}
/* Set multiple channels to the same 12bit value
*/
void PCA9685::PWMSame(uint8_t start, uint8_t numch, uint16_t value)
{
uint16_t newvals [numch];
memset(newvals,0,numch*2);
for(int ch = 0; ch < numch; ch++) {
newvals[ch] = value;
}
PWM(start, numch, newvals);
}
void initialize(void){
SYSTEMConfig(80000000, SYS_CFG_ALL); // sets up periferal and clock configuration
INTEnableSystemMultiVectoredInt();
INTEnableInterrupts(); // enable interrupts
delay();
timers();
delay();
PWM();
delay();
UART();
delay();
beginLIDARdecoder(returned_data, &buffer_five);
}
Motor::Motor(uint32_t freq, float duty, uint8_t TIM_No, uint8_t CH_No,PinTypedef pin) {
pwm_ = PWM(freq, duty, TIM_No, CH_No, pin);
}
void main(void)
{
WDTCTL = WDTPW + WDTHOLD; // Stop WDT
BCSCTL1 = CALBC1_1MHZ; // Set DCO (MPU to 1MHz)
DCOCTL = CALDCO_1MHZ;
P1SEL = BIT1 + BIT2; // P1.1 = RXD, P1.2=TXD
P1SEL2 = BIT1 + BIT2; // P1.1 = RXD, P1.2=TXD
//P1DIR |= BIT0 + BIT6; // BIT0 and BIT6 as outputs (LEDS)
P1OUT = 0;
P2OUT = 0;
UCA0CTL1 |= UCSSEL_2; // SMCLK
UCA0BR0 = 104; // 1MHz 9600
UCA0BR1 = 0; // 1MHz 9600
UCA0MCTL = UCBRS0; // Modulation UCBRSx = 1
UCA0CTL1 &= ~UCSWRST; // **Initialize USCI state machine**
IE2 |= UCA0RXIE; // Enable USCI_A0 RX interrupt
__bis_SR_register(GIE); //interrupts enabled
char text[32];
unsigned int text_lenth = 32;
flushArray(text,text_lenth);
char command[10];
unsigned int command_lenth = 10;
flushArray(command,10);
//*INIT*//
text[0] = 'H';
text[1] = 'E';
text[2] = 'J';
text_lenth = sizeOfArray(text);
//char intext[20];
//flushArray(intext,20);
flushArray(interarray,8);
//char set_name[] = {"AT+NAMEkretsn5"};
//send_at_command(set_name, 14);
//char ret_true[] = {"SUCCESS"};
//char ret_false[] = {"LAME"};
TACCR0 = 20000;
TACCTL0 |= CCIE; // Enable interrupts for CCR0.
TACTL |= MC_1 + TASSEL1 + ID_0 + TACLR;
P1DIR |= red_L + red_R + blue_R + green_R; // LED pins to outputs, BTN is
P2DIR |= blue_L + green_L;
__enable_interrupt();
//boot_seq(); //Awesome boot sequence!
for(;;)
{
on();
PWM();
if(P1IN & button_L && hold_L == 0)
{
dim_all();
mode--;
hold_L = 1;
buttonDelay(250);
}
if(P1IN & button_R && hold_R == 0)
{
dim_all();
mode++;
hold_R = 1;
buttonDelay(250);
}
if((P1IN & ~button_L) && (hold_L == 1))
{
hold_L = 0;
}
if((P1IN & ~button_R) && (hold_R == 1))
{
hold_R = 0;
}
if(f_read == 1)
{
IE2 &= ~UCA0RXIE;
//send_text(interarray,in_lenth);
command_lenth = read_buffer(command);
//*DEBUG*
//send_text(command, command_lenth);
f_read = 0;
IE2 |= UCA0RXIE;
}
if(f_command == 1)
{
IE2 &= ~UCA0RXIE;
if ( strcmp(command, "forwards") == 0 )
{
mode++;
}
else if ( strcmp(command, "backward") == 0 )
{
mode--;
}
else if ( strcmp(command, "mode0000") == 0 )
{
mode = 0;
}
else if ( strcmp(command, "mode0001") == 0 )
{
mode = 1;
}
else if ( strcmp(command, "mode0002") == 0 )
{
mode = 2;
}
else if ( strcmp(command, "mode0003") == 0 )
{
mode = 3;
}
else if ( strcmp(command, "mode0004") == 0 )
{
mode = 4;
}
else if ( strcmp(command, "mode0005") == 0 )
{
mode = 5;
}
else if ( strcmp(command, "mode0006") == 0 )
{
mode = 6;
}
else if ( strcmp(command, "mode0007") == 0 )
{
mode = 7;
chaosTime = 0;
}
else if ( strcmp(command, "mode0008") == 0 )
{
mode = 8;
}
else if ( strcmp(command, "mode0009") == 0 )
{
mode = 9;
}
else if ( strcmp(command, "mode0010") == 0 )
{
mode = 10;
}
else if (command[0] == 'c' && command[1] == 'u' )
{
for(special_iter = 0; special_iter < 6; special_iter++)
{
special[special_iter] = command[special_iter+2];
}
mode = 8;
}
f_command = 0;
flushArray(command,command_lenth);
IE2 |= UCA0RXIE;
}
}
}
void MAIN_PROG (uint16_t time)
{
static uint16_t ticker = 0;
if ((uint16_t)(ticker+time)== GetTicker())
{
ticker = GetTicker();
//Encoder section
ENC_PROCESSING (&Left);
ENC_PROCESSING (&Right);
Linear.velo_raw = (Left.velo_fb - Right.velo_fb)/ 2.0f;
Smooth_filter (Linear.velo_raw,Linear.velo_filted,0.1);
PID_VELO_RUN.feedback = Linear.velo_filted[0];
PID_VELO_HOLD.feedback = Linear.velo_filted[0];
Linear.posi_raw = (Left.posi_fb - Right.posi_fb)/ 2.0f;
Smooth_filter (Linear.posi_raw,Linear.posi_filted,0.1);
PID_POSI.feedback = Linear.posi_filted[0];
/*MPU 1*/
/* Read all data */
MPU6050_Status = TM_MPU6050_ReadAll(&MPU6050_Data0);
//Pitch
/* Complementary filter*/
Pitch.raw = (0.98f * (Pitch.raw - MPU6050_Data0.Gyroscope_Y * dt)) + (0.02f * (atan2f(MPU6050_Data0.Accelerometer_X, MPU6050_Data0.Accelerometer_Z) * 180.0f/PI));
Smooth_filter (Pitch.raw,Pitch.filted_theta,0.25);
PID_TILT.feedback = Pitch.filted_theta[0] - Pitch.theta_offset;
//Yaw
// Yaw.raw = - MPU6050_Data0.Gyroscope_Z;
// /* HighPass filter*/
// Yaw.filted_theta[0] = HPF(Yaw.raw,10,f_s);
// PID_TURN.feedback = Yaw.filted_theta[0] - Yaw.theta_offset;
if ((PID_TILT.feedback < 45) && (PID_TILT.feedback > -45))
{
//Preprocessing
if ((setspeed-PID_VELO_RUN.set_point) > 0)
{
PID_VELO_RUN.set_point += 0.5f;
}
if ((setspeed-PID_VELO_RUN.set_point) < 0)
{
PID_VELO_RUN.set_point -= 0.5f;
}
if (PID_VELO_RUN.set_point == 0)
{
//PID POSITION
PID (&PID_POSI,0.34);
PID_VELO_HOLD.set_point = PID_POSI.filted_U[0] * MAX_VELO;
//PID VELOCITY
PID (&PID_VELO_HOLD,0.34);
PID_TILT.set_point = -PID_VELO_HOLD.filted_U[0] * MAX_TILT;
}
else
{
//PID VELOCITY
PID (&PID_VELO_RUN,0.34);
PID_TILT.set_point = -PID_VELO_RUN.filted_U[0] * MAX_TILT;
}
//PID TILTING
PID (&PID_TILT,0.36);
//Turning
//PID (&PID_TURN,0.12);
U_offset = -setturn*TURN_OFFSET;
//FUSION
Left_motor.PWM = PID_TILT.filted_U[0] + U_offset;
Right_motor.PWM = PID_TILT.filted_U[0] - U_offset;
//PWM section
PWM (&Left_motor);
PWM (&Right_motor);
}
else
{
TIM_SetCompare1(TIM4,0);
TIM_SetCompare2(TIM4,0);
TIM_SetCompare3(TIM4,0);
TIM_SetCompare4(TIM4,0);
GPIO_ResetBits(GPIOD,GPIO_Pin_12);
GPIO_ResetBits(GPIOD,GPIO_Pin_13);
GPIO_ResetBits(GPIOD,GPIO_Pin_14);
GPIO_ResetBits(GPIOD,GPIO_Pin_15);
ResetPID (&PID_TILT);
ResetPID (&PID_VELO_HOLD);
ResetPID (&PID_VELO_RUN);
ResetPID (&PID_POSI);
}
}
}
/**
* Single channel 8bit PWM only. Slow in loop
*/
void PCA9685::PWM8(uint8_t channel, uint8_t value)
{
PWM(channel, CIELPWM(value));
}
/**
* Single channel 12bit PWM only. Slow in loop
*/
void PCA9685::PWM(uint8_t channel, uint16_t value)
{
PWM(channel, 1, &value);
}
