Romain Ame / Mbed 2 deprecated Robot2016_2-0_STATIC

Voili voilou

Dependencies:   RoboClaw StepperMotor mbed

Fork of Robot2016_2-0 by ARES

main.cpp@14:d5e21f71c2a9, 2016-01-05 (annotated)

Committer:
sype
Date:
Tue Jan 05 14:51:10 2016 +0000
Revision:
14:d5e21f71c2a9
Parent:
10:ae3178aa94e9
Child:
16:a1ede21a963b
Old odo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sype 0:ad9600df4a70 1 #include "Odometry.h"
sype 0:ad9600df4a70 2
sype 0:ad9600df4a70 3 #define dt 10000
sype 10:ae3178aa94e9 4 #define ATTENTE 0
sype 10:ae3178aa94e9 5 #define GO 1
sype 10:ae3178aa94e9 6 #define STOP 2
sype 0:ad9600df4a70 7
sype 10:ae3178aa94e9 8 InterruptIn mybutton(USER_BUTTON);
sype 10:ae3178aa94e9 9 DigitalIn button(USER_BUTTON);
sype 2:abdf8c6823a1 10 DigitalOut led(LED1);
sype 0:ad9600df4a70 11 Ticker ticker;
sype 10:ae3178aa94e9 12 //Serial pc(USBTX, USBRX);
sype 10:ae3178aa94e9 13 Serial pc(PA_9, PA_10);
sype 10:ae3178aa94e9 14 RoboClaw roboclaw(460800, PA_11, PA_12);
sype 10:ae3178aa94e9 15 Odometry odo(63.2, 63.3, 252, 4096, roboclaw);
sype 10:ae3178aa94e9 16
sype 10:ae3178aa94e9 17 int i = 0;
sype 0:ad9600df4a70 18
sype 0:ad9600df4a70 19 void update_main(void);
sype 2:abdf8c6823a1 20 void init(void);
sype 10:ae3178aa94e9 21 void pressed(void);
sype 10:ae3178aa94e9 22 void unpressed(void);
sype 0:ad9600df4a70 23
IceTeam 9:e39b218ab20d 24 /** Debut du programme */
sype 0:ad9600df4a70 25 int main(void)
sype 10:ae3178aa94e9 26 {
sype 2:abdf8c6823a1 27 init();
sype 14:d5e21f71c2a9 28 //roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, -116878, accel_angle, vitesse_angle, deccel_angle, 116878, 1);
sype 14:d5e21f71c2a9 29 odo.GotoXY(1800,1500);
sype 14:d5e21f71c2a9 30 odo.GotoXY(2500,500);
sype 14:d5e21f71c2a9 31 odo.GotoXY(2000,300);
sype 14:d5e21f71c2a9 32 odo.GotoXYT(300,1000,0);
sype 14:d5e21f71c2a9 33 //for(int n=0; n<40; n++) odo.setTheta();
sype 14:d5e21f71c2a9 34 odo.GotoThet(-PI);
sype 14:d5e21f71c2a9 35 odo.GotoThet(0);
sype 10:ae3178aa94e9 36 wait(2000);
sype 2:abdf8c6823a1 37 while(1);
sype 2:abdf8c6823a1 38 }
sype 2:abdf8c6823a1 39
sype 2:abdf8c6823a1 40 void init(void)
sype 0:ad9600df4a70 41 {
sype 10:ae3178aa94e9 42 pc.baud(9600);
sype 2:abdf8c6823a1 43 pc.printf("Hello from main !\n\r");
sype 10:ae3178aa94e9 44 wait_ms(500);
sype 10:ae3178aa94e9 45
sype 14:d5e21f71c2a9 46 odo.setPos(300, 1000, 0);
sype 2:abdf8c6823a1 47 roboclaw.ForwardM1(ADR, 0);
sype 2:abdf8c6823a1 48 roboclaw.ForwardM2(ADR, 0);
sype 10:ae3178aa94e9 49
sype 10:ae3178aa94e9 50 while(button);
sype 10:ae3178aa94e9 51 wait(1);
sype 10:ae3178aa94e9 52 mybutton.fall(&pressed);
sype 10:ae3178aa94e9 53 mybutton.rise(&unpressed);
sype 0:ad9600df4a70 54 ticker.attach_us(&update_main,dt); // 100 Hz
sype 0:ad9600df4a70 55 }
sype 0:ad9600df4a70 56
sype 0:ad9600df4a70 57 void update_main(void)
sype 0:ad9600df4a70 58 {
sype 0:ad9600df4a70 59 odo.update_odo();
sype 10:ae3178aa94e9 60 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 14:d5e21f71c2a9 61 //pc.printf("Theta : %3.2f\n\r", odo.getTheta()*180/PI);
sype 14:d5e21f71c2a9 62 pc.printf("X : %4.2f\n\r", odo.getX());
sype 2:abdf8c6823a1 63 //if(pc.readable()) if(pc.getc()=='l') led = !led;
sype 0:ad9600df4a70 64 }
sype 10:ae3178aa94e9 65
sype 10:ae3178aa94e9 66 void pressed(void)
sype 10:ae3178aa94e9 67 {
sype 10:ae3178aa94e9 68 if(i==0) {
sype 10:ae3178aa94e9 69 roboclaw.ForwardM1(ADR, 0);
sype 10:ae3178aa94e9 70 roboclaw.ForwardM2(ADR, 0);
sype 10:ae3178aa94e9 71 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 10:ae3178aa94e9 72 i++;
sype 10:ae3178aa94e9 73 }
sype 10:ae3178aa94e9 74 }
sype 10:ae3178aa94e9 75
sype 10:ae3178aa94e9 76 void unpressed(void)
sype 10:ae3178aa94e9 77 {
sype 10:ae3178aa94e9 78 if(i==1) {
sype 10:ae3178aa94e9 79 i--;
sype 10:ae3178aa94e9 80 }
sype 10:ae3178aa94e9 81 }