Base::Base() :
Element()
{
float motorDistance = 46.188;
// Positions des moteurs par rapport au referenciel des moteurs
Vec3f motor1(sin(deg2Rad(36.28f))*motorDistance, cos(deg2Rad(36.28f))
*motorDistance, 0);
Vec3f motor3(-(cos(deg2Rad(36.28f-30.0f))*motorDistance),
sin(deg2Rad(36.28f-30.0f))*motorDistance, 0);
Vec3f motor2(sin(deg2Rad(60.0f-36.28f))*motorDistance, -(cos(deg2Rad(60.0f
-36.28f))*motorDistance), 0);
osg::Node* node = osgDB::readNodeFile("data/base.stl");
node->setStateSet(createTransparency(0.5));
// Centering the model
osg::MatrixTransform* modelTransform = new osg::MatrixTransform();
modelTransform->setMatrix(osg::Matrix::translate(0, 0, -5));
modelTransform->addChild(node);
osg::MatrixTransform* transform = new osg::MatrixTransform();
transform->addChild(modelTransform);
transform->setMatrix(osg::Matrix::translate( 0, 0, 0));
model->addChild(transform);
}
int main() {
usartInit(57600);
usartEnableReceiver();
usartEnableTransmitter();
usartStdio();
setBit(DDRB, M1_DIR);
setBit(DDRD, M1_PWM);
setBit(DDRD, M2_DIR);
setBit(DDRD, M2_PWM);
clrBit(PORTB, M1_DIR);
clrBit(PORTD, M1_PWM);
clrBit(PORTD, M2_PWM);
clrBit(PORTD, M2_DIR);
timer0FastPWMMaxMode();
timer0ClockPrescaller64();
timer0OC0ANonInvertedMode();
timer0OC0BNonInvertedMode();
timer0SetCompareAValue(0);
timer0SetCompareBValue(0);
timer0DeactivateCompareAInterrupt();
timer0DeactivateCompareBInterrupt();
timer2ClockPrescaller256();
timer2NormalMode();
timer2OC2AOff();
timer2OC2BOff();
timer2ActivateOverflowInterrupt();
i2c_init();
adxl345_init();
l3g4200d_init();
l3g4200d_setoffset(0.11, -1.71, -0.46);
//l3g4200d_settemperatureref();
PID_init(&PID, 15,0.2,1, 0);
PID_SetMode(&PID, 1);
PID_SetOutputLimits(&PID, -255, 255);
PID.mySetpoint = 0;
motor1(0);
motor2(0);
// printf("\e[1;1H\e[2J");
// printf("STARTED!\r\n");
// _delay_ms(200);
// motor1(120);
// printf("Motor1 = 120\r\n");
// _delay_ms(500);
// motor1(-120);
// printf("Motor1 = -120\r\n");
// _delay_ms(500);
// motor1(120);
// printf("Motor1 = 120\r\n");
// _delay_ms(500);
// motor1(0);
// printf("Motor1 = 0\r\n");
sei();
while(1);
return 0;
}
void remote_controlled(void)
{
/*¨Parts:
-Leds (L)
-Motors (M)
-servos (S)
-complete movement (R) */
char part;
int opt1=0;
int opt2=0;
int opt3=0;
//scanf("%1c%1i%3i%3i",&part,&opt1,&opt2,&opt3);
scanf("%1c%1i%3i",&part,&opt1,&opt2);
switch (part)
{
/* Motors M+#of motor + speed (99 to -99) */
case 'M':
switch (opt1)
{
case 1:
motor1(opt2);
break;
case 2:
motor2(opt2);
break;
case 3:
motor3(opt2);
break;
case 4:
motor4(opt2);
break;
}
break;
case 'R':
move(opt2,opt3);
break;
case 'S':
switch(opt1)
{
case 1:
pwm_update(SERVO_0,opt2);
break;
case 2:
pwm_update(SERVO_1,opt2);
break;
case 3:
pwm_update(SERVO_2,opt2);
break;
case 4:
pwm_update(SERVO_3,opt2);
break;
}
break;
/* Leds, L + # of led + option
L1000 turn off led 1
L1001 turn on led 1
L1002 toggle led 1 */
case 'L':
switch (opt1)
{
case 1:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED1,0);
break;
case 1:
ioport_set_pin_level(LED1,1);
break;
case 2:
ioport_toggle_pin_level(LED1);
break;
}
break;
case 2:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED2,0);
break;
case 1:
ioport_set_pin_level(LED2,1);
break;
case 2:
ioport_toggle_pin_level(LED2);
break;
}
break;
case 3:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED3,0);
break;
case 1:
ioport_set_pin_level(LED3,1);
break;
case 2:
ioport_toggle_pin_level(LED3);
break;
}
break;
case 4:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED4,0);
break;
case 1:
ioport_set_pin_level(LED4,1);
break;
case 2:
ioport_toggle_pin_level(LED4);
break;
}
break;
case 5:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED5,0);
break;
case 1:
ioport_set_pin_level(LED5,1);
break;
case 2:
ioport_toggle_pin_level(LED5);
break;
}
break;
case 6:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED6,0);
break;
case 1:
ioport_set_pin_level(LED6,1);
break;
case 2:
ioport_toggle_pin_level(LED6);
break;
}
break;
}
// case 'M':
// break;
//
// case 'S':
// break;
//
// case 'R':
// break;
}
}
void set_rgb_led(char r, char g, char b){
motor0(r);
motor1(g);
motor2(b);
}
void motor2toe(){
motor2(1,3);
}
void motor2open(){
motor2(0,3);
}
