Revision 65:4709ff6c753c, committed 2013-04-14

Comitter:

rsavitski

Date:

Sun Apr 14 17:57:12 2013 +0000

Parent:

59:63609922579c

Child:

67:be3ea5450cc7

Child:

68:662164480f60

Commit message:

rewrote arm code, pushes top layer with uber paddle

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuators/Arms/Arm.cpp	Sun Apr 14 17:57:12 2013 +0000
@@ -0,0 +1,9 @@
+#include "Arm.h"
+
+namespace arm
+{
+
+Arm lower_arm(p25, 0.05, 0.9, 2.0);
+Arm upper_arm(p26, 0.05, 0.6, 2.5);
+
+} //namespace
\ No newline at end of file

--- a/Actuators/Arms/Arm.h	Sat Apr 13 22:42:01 2013 +0000
+++ b/Actuators/Arms/Arm.h	Sun Apr 14 17:57:12 2013 +0000
@@ -1,69 +1,44 @@
-
-// Eurobot13 WhiteArm.h
+#ifndef EUROBOT_ACTUATORS_ARMS_ARM_H_
+#define EUROBOT_ACTUATORS_ARMS_ARM_H_
 
 #include "mbed.h"
-#include "Servo.h"
 
-class Arm : public Servo
+namespace arm
 {
-private:
-    bool updirn;
+
+class Arm;
 
+extern Arm lower_arm;
+extern Arm upper_arm;
+
+
+class Arm 
+{
 public:
-    Arm ( PinName yellowPWM
-        , bool upflip = false
-        , float range = 0.0005, float degrees = 45.0
-        ) 
-        : Servo(yellowPWM) 
-        {
-        calibrate(range, degrees);
-        updirn = upflip;
+    Arm (PwmOut pwm_in, float period_in, float min_pos_in, float max_pos_in)
+       : pwm_(pwm_in), period_(period_in), min_pos_(min_pos_in), max_pos_(max_pos_in)
+    {
+        pwm_.period(period_);
     }
 
-    void operator()(float in) {
-        write(in);
-    }
-
-    void clockwise() { // full lock clockwise
-        write(updirn?1:0);
-    }
-
-    void anticlockwise() { // full lock anticlockwise
-        write(updirn?0:1);
+    void go_up()
+    {
+        pwm_.pulsewidth_us(max_pos_*1000);
     }
     
-    virtual void halt() { // servo applies no force
-        //DigitalOut myled(LED3);
-        //myled = 1;
-        _pwm = 0;
-    }
-};
-
-
-/*
-class Servo{
-    private:
-    PwmOut PWM;
-
-    public:
-    Servo(PinName pin1) : PWM(pin1){
+    void go_down()
+    {
+        pwm_.pulsewidth_us(min_pos_*1000);
     }
 
-    void operator()(float in){
-        PWM = in;
-    }
-
-    void clockwise() { // full lock clockwise
-        PWM = .135;
-    }
-
-    void anticlockwise() { // full lock anticlockwise
-        PWM = .025;
-    }
-
-    void relax() { // servo applies no force
-        PWM = 0;
-    }
+private:
+    PwmOut pwm_;
+    float period_;
+    float min_pos_;
+    float max_pos_;
 
 };
-*/
\ No newline at end of file
+
+} //namespace
+
+#endif
\ No newline at end of file

--- a/Actuators/Arms/Servo.lib	Sat Apr 13 22:42:01 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/simon/code/Servo/#36b69a7ced07

--- a/Processes/AI/ai.cpp	Sat Apr 13 22:42:01 2013 +0000
+++ b/Processes/AI/ai.cpp	Sun Apr 14 17:57:12 2013 +0000
@@ -4,6 +4,7 @@
 #include "motion.h"
 #include "Colour.h"
 #include "supportfuncs.h"
+#include "Arm.h"
 
 
 namespace AI
@@ -17,20 +18,26 @@
     current_waypoint.y = 1.85;
     current_waypoint.theta = PI;
     current_waypoint.pos_threshold = 0.01;
-    current_waypoint.angle_threshold = 0.02*PI;
+    current_waypoint.angle_threshold = 0.01*PI;
 
     motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
 
     Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
     Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
 
-    float r = 0.61+0.02;
+    float r = 0.61+0.02+0.03;
 
     for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
     {
         motion::waypoint_flag_mutex.lock();
-        if (motion::checkMotionStatus() && c_lower.getColour()==RED)
+        if (motion::checkMotionStatus() && c_upper.getColour()==RED)
         {
+            //temphack!!!!!
+            Thread::wait(1000);
+            arm::upper_arm.go_down();
+            Thread::wait(2000);
+            arm::upper_arm.go_up();
+            
             phi -= 22.5/180*PI;
             current_waypoint.x = 1.5-r*cos(phi);
             current_waypoint.y = 2-r*sin(phi);

--- a/Processes/Motion/motion.cpp	Sat Apr 13 22:42:01 2013 +0000
+++ b/Processes/Motion/motion.cpp	Sun Apr 14 17:57:12 2013 +0000
@@ -103,9 +103,7 @@
             wp_a_reached = true;
         }
     } 
-    waypoint_flag_mutex.unlock();
-        
-    waypoint_flag_mutex.lock(); // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag
+     // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag
     if (wp_d_reached && wp_a_reached)
     {
         setWaypointReached();

--- a/Processes/Printing/Printing.h	Sat Apr 13 22:42:01 2013 +0000
+++ b/Processes/Printing/Printing.h	Sun Apr 14 17:57:12 2013 +0000
@@ -1,7 +1,7 @@
 
 // Eurobot13 Printing.h
 
-//#define PRINTINGOFF
+#define PRINTINGOFF
 
 #include "mbed.h"
 #include "rtos.h"

--- a/main.cpp	Sat Apr 13 22:42:01 2013 +0000
+++ b/main.cpp	Sun Apr 14 17:57:12 2013 +0000
@@ -39,8 +39,8 @@
     //motorencodetest();
     //motorencodetestline();
     //motorsandservostest();
-    armtest();
-    while(1);
+    //armtest();
+    //while(1);
     //motortestline();
     //ledtest();
     //phototransistortest();
@@ -245,22 +245,74 @@
     }
 }
 */
+#ifdef AGDHGADSYIGYJDGA
+PwmOut white(p26);
+PwmOut black(p25);
 
-
+void armtest() 
+{    
+    
+    white.period(0.05);
+    black.period(0.05);
+    
+  /*  float f=1;
+    for (f=1; f<3; f+=0.1)
+    { 
+        black.pulsewidth_us(f*1000);
+        wait(1);
+        printf("%f\r\n", f);
+    }
+    for (f=2; f>0; f-=0.1)
+    { 
+        black.pulsewidth_us(f*1000);
+        wait(1);
+        printf("%f\r\n", f);
+    }*/
+    
+    
+    for(;;)
+    {
+        black.pulsewidth_us(2.0*1000);
+        wait(2);
+        black.pulsewidth_us(0.9*1000);//1.2
+        wait(2);
+    }
+    
+    // white works
+    /*for(;;)
+    {
+        white.pulsewidth_us(0.6*1000);
+        wait(2);
+        white.pulsewidth_us(2.5*1000);
+        wait(2);
+    }*/
+    
+   /* while(1) //2.5 -> //0.6
+    {
+        white.pulsewidth_us(int(f*1000));
+        printf("%f\r\n", f);
+        f-=0.1;
+        wait(1);
+    }*/
+}
+#endif
+#ifdef FSDHGFSJDF
 void armtest()
 {
-    Arm white(p26), black(p25, false, 0.0005, 180);
-    while(true) {
-        white(0);
-        black(0.2);
-        wait(1);
-        white(1);
-        black(0.8);
-        wait(1);
+    Arm lower_arm(p25, 0.05, 0.9, 2.0);
+    Arm upper_arm(p26, 0.05, 0.6, 2.5);
+    
+    while(1)
+    {
+        upper_arm.go_up();
+        wait(2);
+        upper_arm.go_down();
+        wait(2);
     }
 }
-
-
+#endif
+void armtest(){}
+/*
 void motorsandservostest()
 {
     Encoder Eleft(p27, p28), Eright(p30, p29);
@@ -301,7 +353,7 @@
         if (servoTimer.read() >= 9) servoTimer.reset();
     }
 }
-
+*/
 void motortestline()
 {
     MainMotor mleft(p24,p23), mright(p21,p22);