ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

Files at this revision

API Documentation at this revision

Revision 37:da3a2c781672, committed 2016-02-06

Comitter:
sype
Date:
Sat Feb 06 10:11:28 2016 +0000
Parent:
36:2d7357a385bc
Child:
39:309f38d1e49c
Child:
40:ca4dd3faffa8
Commit message:
Command;

Changed in this revision

Odometry/Odometry.cpp Show annotated file Show diff for this revision Revisions of this file
Odometry/Odometry.h Show annotated file Show diff for this revision Revisions of this file
RoboClaw.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/Odometry/Odometry.cpp	Sun Jan 31 16:11:32 2016 +0000
+++ b/Odometry/Odometry.cpp	Sat Feb 06 10:11:28 2016 +0000
@@ -28,7 +28,7 @@
 }
 void Odometry::getEnc()
 {
-    pc.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR));
+    logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR));
 }
 
 void Odometry::setX(double x)
@@ -91,7 +91,7 @@
 {
     double theta_ = atan2(y_goal-y, x_goal-x);
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
-    pc.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
+    logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoThet(theta_);
     GotoDist(dist_);
 }
@@ -100,7 +100,7 @@
 {
     double theta_ = atan2(y_goal-y, x_goal-x);
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
-    pc.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
+    logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoThet(theta_);
     GotoDist(dist_);
     GotoThet(theta_goal);
@@ -110,7 +110,7 @@
 {
     led = 0;
     //pos_prog++;
-    //pc.printf("Theta : %3.2f\n\r", theta_*180/PI);
+    //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
 
     int32_t distance_ticks_left;
@@ -124,7 +124,7 @@
     while(erreur_theta >= PI) erreur_theta -= 2*PI;
     while(erreur_theta < -PI) erreur_theta += 2*PI;
     
-    pc.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
+    logger.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
     
     if(erreur_theta < 0) {
         distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
@@ -134,27 +134,27 @@
         distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
     }
 
-    //pc.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
-    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
-    //pc.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
+    //logger.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
+    //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
+    //logger.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
 
     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
 
-    //pc.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
+    //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
 
-    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //pc.printf("%6d\t%6d\t%6d\t%6d\t%6d\n\r",m_pulses_right - pos_initiale_right, distance_ticks_right, m_pulses_left - pos_initiale_left, distance_ticks_left);
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("%6d\t%6d\t%6d\t%6d\t%6d\n\r",m_pulses_right - pos_initiale_right, distance_ticks_right, m_pulses_left - pos_initiale_left, distance_ticks_left);
     wait(0.4);
     setTheta(theta_);
     led = 1;
     //arrived = true;
-    //pc.printf("arrivey %d\n\r",pos_prog);
+    //logger.printf("arrivey %d\n\r",pos_prog);
 }
 
 void Odometry::GotoDist(double distance)
 {
     led = 0;
     //pos_prog++;
-    //pc.printf("Dist : %3.2f\n\r", distance);
+    //logger.printf("Dist : %3.2f\n\r", distance);
     //arrived = false;
 
     int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
@@ -164,13 +164,13 @@
 
     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
 
-    //pc.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
+    //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
     
-    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //pc.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
     wait(0.4);
     led = 1;
-    //pc.printf("arrivey %d\n\r",pos_prog);
-    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
+    //logger.printf("arrivey %d\n\r",pos_prog);
+    //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
 }
 
 void Odometry::TestEntraxe(int i) {

--- a/Odometry/Odometry.h	Sun Jan 31 16:11:32 2016 +0000
+++ b/Odometry/Odometry.h	Sat Feb 06 10:11:28 2016 +0000
@@ -20,7 +20,7 @@
 *   Author : Benjamin Bertelone, reworked by Simon Emarre
 */
 
-extern Serial pc;
+extern Serial logger;
 extern DigitalOut led;
 
 /** Permet la gestion de l'odometrie du robot **/

--- a/RoboClaw.lib	Sun Jan 31 16:11:32 2016 +0000
+++ b/RoboClaw.lib	Sat Feb 06 10:11:28 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/ARES/code/RoboClaw/#e64524a61cfe
+https://developer.mbed.org/teams/ARES/code/RoboClaw/#ad00b3431ff5

--- a/main.cpp	Sun Jan 31 16:11:32 2016 +0000
+++ b/main.cpp	Sat Feb 06 10:11:28 2016 +0000
@@ -12,10 +12,8 @@
 DigitalOut led(LED1);
 Ticker ticker;
 //Serial pc(USBTX, USBRX);
-Serial pc(PA_9, PA_10);
 Serial logger (PA_9, PA_10);
 RoboClaw roboclaw(460800, PA_11, PA_12);
-/* Changement entraxe : 252->253 */
 Odometry odo(63.2, 63.3, ENTRAXE, 4096, roboclaw);
 
 int i = 0;
@@ -28,12 +26,13 @@
 /** Debut du programme */
 int main(void)
 {
-    double angle_v = 2*PI/5;
+    init();
+    /*double angle_v = 2*PI/5;
     double distance_v = 200.0;
     std::vector<SimplePoint> path;
     Map map;
     
-    init();
+    
     //Construction des obstacles
     map.build();
     
@@ -60,31 +59,57 @@
     //odo.GotoThet(PI);
     odo.GotoThet(0);
     //odo.TestEntraxe(3);
-   /*
-    odo.GotoXYT(1000, 1000, 0);
-    odo.GotoXYT(0, 1000, PI);
-    odo.GotoThet(0);*/
     
     //odo.GotoThet(-PI/2);
     wait(2000);
-    //odo.GotoXYT(2250,500,0);
-    while(1);
+    //odo.GotoXYT(2250,500,0);*/
+    while(1)
+    {
+        while(logger.readable()) 
+        {
+            char c = logger.getc();
+            if(c=='z')
+            {
+                roboclaw.SpeedAccelM1M2(ADR, accel_angle, speed_angle, accel_angle, speed_angle);
+            }
+            else if(c == 's')
+            {
+                roboclaw.ForwardM1(ADR, 0);
+                roboclaw.ForwardM2(ADR, 0);
+            }
+            else if(c == 'd')
+            {
+                roboclaw.SpeedAccelM1M2(ADR, accel_angle, -speed_angle, accel_angle, speed_angle);
+            }
+            else if(c == 'q')
+            {
+                roboclaw.SpeedAccelM1M2(ADR, accel_angle, speed_angle, accel_angle, -speed_angle);
+            }
+            else if(c == 'x')
+            {
+                roboclaw.SpeedAccelM1M2(ADR, accel_angle, -speed_angle, accel_angle, -speed_angle);
+            }
+        }
+        roboclaw.ForwardM1(ADR, 0);
+        roboclaw.ForwardM2(ADR, 0);
+    }
 }
 
 void init(void)
 {
-    pc.baud(9600);
-    pc.printf("Hello from main !\n\r");
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    logger.baud(9600);
+    logger.printf("Hello from main !\n\r");
     wait_ms(500);
     
     odo.setPos(0, 1000, 0);
-    roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
-    
+        
     while(button);
     wait(1);
     mybutton.fall(&pressed);
-    mybutton.rise(&unpressed);
+    mybutton.rise(&unpr
+    essed);
     ticker.attach_us(&update_main,dt); // 100 Hz
 }