MbedResponse *NetCentricApp::rotateR(MbedRequest *request) {
printf("---------------IK BEN HIER___________");
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
if (request->n > 0) led1 = request->args[0];
if (request->n > 1) led2 = request->args[1];
if (request->n > 2) led3 = request->args[2];
if (request->n > 3) led4 = request->args[3];
MbedResponse *r = new MbedResponse();
r->requestId = request->id;
r->commandId = request->commandId;
r->error = NO_ERROR;
r->n = 4;
r->values = new float[4];
r->values[0] = led1;
r->values[1] = led2;
r->values[2] = led3;
r->values[3] = led4;
callServo(request->args[0]);
return r;
}
// Control the LED's.
MbedResponse *NetCentricApp::ledCommand(MbedRequest *request) {
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
if (request->n > 0) led1 = request->args[0];
if (request->n > 1) led2 = request->args[1];
if (request->n > 2) led3 = request->args[2];
if (request->n > 3) led4 = request->args[3];
MbedResponse *r = new MbedResponse();
r->requestId = request->id;
r->commandId = request->commandId;
r->error = NO_ERROR;
r->n = 4;
r->values = new float[4];
r->values[0] = led1;
r->values[1] = led2;
r->values[2] = led3;
r->values[3] = led4;
return r;
}
void leds_state(int leds)
{
led1( ( leds & (1<<0) ) && (1<<0) );
led2( ( leds & (1<<1) ) && (1<<1) );
led3( ( leds & (1<<2) ) && (1<<2) );
led4( ( leds & (1<<3) ) && (1<<3) );
led5( ( leds & (1<<4) ) && (1<<4) );
led6( ( leds & (1<<5) ) && (1<<5) );
}
main() {
pc.baud(115200); //set baud rate to 115200
Point2d temp;
Point2f curr;
bool fresh = false;
// predicted position of the ball
Point2d pixel_pred;
int time_to_fwd = INT_MAX;
int time_to_def = INT_MAX;
/*PinName MotL_en, PinName MotL_fw, PinName MotL_re, PinName MotL_Qa, PinName MotL_Qb,
PinName MotR_en, PinName MotR_fw, PinName MotR_re, PinName MotR_Qa, PinName MotR_Qb*/
Rod rod_fwd(p22, p27, p28, p29, p30,
p23, p15, p16, p17, p18);
Rod rod_def(p24, p5, p6, p7, p8,
p25, p9, p10, p11, p12); // initialise the two rods
Tracker t;
DigitalOut led3(LED3);
int rod_to_move;
Timer timer;
timer.start();
led3 = 0;
Point2d prev_temp;
while (1) {//infinite while loop
if(timer.read_ms()>=PERIOD){
timer.reset();
led3 = !led3;
curr.x = curr.y = -1;
if(get_x_and_y(temp.x,temp.y)) {//simple logic to determine which rod to activate, time_to_def and time_to_fwd determine urgency
curr.x = temp.x;
curr.y = temp.y;
pixel_pred.y = temp.y;
}
if(curr.x >= ROD_FWD_X) {
time_to_fwd = 20;
time_to_def = INT_MAX;
}
else {
time_to_def = 20;
time_to_fwd = INT_MAX;
}
fresh = true;//set fresh to true to indicate new ball position reading
}
rod_fwd.take_action(pixel_pred.y, time_to_fwd, fresh);//call both rods to run control loops and determine position to move
rod_def.take_action(pixel_pred.y, time_to_def, fresh);
fresh = false;//set fresh to false to indicate stale readings from now on until new updates
}
}
int main(void)
{
//Hier wird alles initialisiert, dh Grundeinstellungen festgelegt
init_Motor(); //Motor einstellen
init_system_tick(); //System Tick einstellen
init_drehzahlsensor(); //Drehzahlsensor einstellen
init_leds(); //LEDs einstellen
init_hupe(); //Hupe einstellen
sei(); //Alle Interrupts einschalten
while(1) //Alles innerhalb der while Schleife wird immer wieder wiederholt
{
if (~PINB & 0x01) //Wenn der invertierte Wert des ersten Bits in PINB 1 ist, dann (also wenn der Button gedrückt wird)
{
led1(AN); //Mach die LEDs 1 und 3 an
led3(AN);
m_r(255,0); //Und die Motoren mit Vollgas (255) in 2 Richtungen
m_l(255,1);
_delay_ms(500); //Warte 500 ms
led1(AUS); //LEDs 1 und 3 aus, LEDs 2 und 4 an
led3(AUS);
led2(AN);
led4(AN);
m_r(255,1); //Richtungen ändern
m_l(255,0);
_delay_ms(500); //und wieder 500ms in die andere Richtung
m_r(0,0); //Motoren aus
m_l(0,0);
led2(AUS); //Leds aus
led4(AUS);
hupe(AN); //Hupe an
_delay_ms(1000); //1 s warten
hupe(AUS); //Hupe aus
_delay_ms(300); //300ms warten
hupe(AN); //Hupe an
_delay_ms(500); //warte 500 ms
hupe(AUS); //Hupe aus
} //wieder nach oben
}
}
void animation(void)
{
volatile int i;
for(i = 50;i<ANIMATION_MAX;i+=15)
{
led1(ON);
delay_ms(i);
led2(ON);
delay_ms(i);
led3(ON);
delay_ms(i);
led4(ON);
delay_ms(i);
led1(OFF);
delay_ms(i);
led2(OFF);
delay_ms(i);
led3(OFF);
delay_ms(i);
led4(OFF);
delay_ms(i);
}
}
//zero_wheel:
void zero_wheel (void)
{
uint d, f=0, t, x;
sto = t1tick + 3000;
stepper_enable (ON);
read_wheel ();
set_ms (100); // full steps by default
t = abs(wheel_center - wheel);
x = 10;
while (x && (sto > t1tick)) {
ClrWdt ();
check_uart1 ();
read_wheel ();
// led3 ((t1tick & 0x40) ? ON : OFF);
t = abs(wheel_center - wheel);
if (t < 15) {
set_ms (t);
}
d = (wheel > wheel_center) ? 1 : 0;
if (t1tick & 4) {
if (!f) {
if (wheel != wheel_center) {
step (d);
}
f = 1;
if (t < 3) {
x--;
}
else {
x = 10;
}
}
}
else if (!(t1tick & 4)) {
f = 0;
}
} // while
if (sto <= t1tick) { // timeout
lpmessage (wheelerr_msg);
}
if ((state != STATE_SERVICE) || !(hold_flag & 1)) {
stepper_enable (OFF);
}
led3 (ON);
wheel_count = wheel;
wc = 0;
}
void vCTaskFunction( void *pvParameters )
{
Serial *usb=(Serial*)pvParameters;
Led led3(LED3);
portTickType lastTickTime;
lastTickTime = xTaskGetTickCount ();
for( ;; )
{
led3.on();
vTaskDelayUntil (&lastTickTime, 25);
led3.off();
usb->printf("** [C] TICK: %d\r\n",(int)lastTickTime);
vTaskDelayUntil (&lastTickTime, 400);
}
}
static void appTaskLeds(void *pdata) {
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
/* Task main loop */
while (true) {
if (led1Flashing) {
led1 = !led1;
}
if (led2Flashing) {
led2 = !led2;
}
if (led3Flashing) {
led3 = !led3;
}
if (led4Flashing) {
led4 = !led4;
}
OSTimeDlyHMSM(0,0,0,500);
}
}
int main (void)
{
ioport_set_pin_dir(IO_1,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_2,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_3,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_4,IOPORT_DIR_OUTPUT);
init_octopus();
initial_animation();
pwm_ini(SERVO_0);
pwm_update(SERVO_0,0);
delay_ms(1000);
pwm_update(SERVO_0,500);
delay_ms(1000);
pwm_update(SERVO_0,1000);
char aux;
char aux2;
int dimmer=0;
for(;;)
{
if(bluetooth_is_rx_complete())
{
scanf("%1c",&aux);
switch (aux)
{
case 'C':
scanf("%1c",&aux);
if (aux=='1')
{
led1(ON);
ioport_set_pin_level(IO_1,ON);
}
else
{
led1(OFF);
ioport_set_pin_level(IO_1,OFF);
}
break;
case 'T':
scanf("%1c",&aux);
if (aux=='1')
{
led2(ON);
ioport_set_pin_level(IO_2,ON);
}
else
{
led2(OFF);
ioport_set_pin_level(IO_2,OFF);
}
break;
case 'D':
scanf("%i",&dimmer);
break;
case 'O':
scanf("%1c",&aux);
scanf("%1c",&aux);
scanf("%1c",&aux2);
if(aux == '1')
{
if(aux2=='1')
{
led3(ON);
ioport_set_pin_level(IO_3,ON);
}
else
{
led3(OFF);
ioport_set_pin_level(IO_3,OFF);
}
}
else
{
if(aux2=='1')
{
led4(ON);
ioport_set_pin_level(IO_4,ON);
}
else
{
led4(OFF);
ioport_set_pin_level(IO_4,OFF);
}
}
break;
}
}
dimmer=dimmer*10;
pwm_update(SERVO_0,dimmer);
}
}
int main (void)
{
ioport_set_pin_dir(IO_1,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_2,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_3,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_4,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_5,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_6,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_7,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_8,IOPORT_DIR_INPUT);
init_octopus();
animation();
//Activate button interrupt
pmic_init();
pmic_set_scheduling(PMIC_SCH_ROUND_ROBIN);
PORTC.INTCTRL = 0x03;
PORTC.INT0MASK = (1<<3);
PORTC.PIN3CTRL |= (1<<1);
cpu_irq_enable();
//Activate ZCD signal interruption
cpu_irq_disable();
PORTD.INTCTRL = 0x03;
PORTD.INT0MASK = (1<<5);
PORTD.PIN5CTRL |= 0x01;
cpu_irq_enable();
//Activate RTC
rtc_init();
// pwm_ini(SERVO_0);
// pwm_update(SERVO_0,0);
// delay_ms(1000);
// pwm_update(SERVO_0,500);
// delay_ms(1000);
// pwm_update(SERVO_0,1000);
char aux;
char aux2;
for(;;)
{
printf("hello");
if(bluetooth_is_rx_complete())
{
scanf("%1c",&aux);
switch (aux)
{
case 'C':
scanf("%1c",&aux);
if (aux=='1')
{
led1(ON);
ioport_set_pin_level(IO_1,ON);
}
else
{
led1(OFF);
ioport_set_pin_level(IO_1,OFF);
}
break;
case 'T':
scanf("%1c",&aux);
if (aux=='1')
{
led2(ON);
ioport_set_pin_level(IO_2,ON);
}
else
{
led2(OFF);
ioport_set_pin_level(IO_2,OFF);
}
break;
case 'D':
scanf("%i",&dimmer);
RTC.COMP=dimmer;
break;
case 'O':
scanf("%1c",&aux);
scanf("%1c",&aux);
scanf("%1c",&aux2);
if(aux == '1')
{
if(aux2=='1')
{
led3(ON);
ioport_set_pin_level(IO_3,ON);
}
else
{
led3(OFF);
ioport_set_pin_level(IO_3,OFF);
}
}
else
{
if(aux2=='1')
{
led4(ON);
ioport_set_pin_level(IO_4,ON);
}
else
{
led4(OFF);
ioport_set_pin_level(IO_4,OFF);
}
}
break;
}
}
}
}