Revision 5:3d88f7506cd9, committed 2016-10-28

Comitter:

meikefrok

Date:

Fri Oct 28 09:01:41 2016 +0000

Parent:

4:e62a2df0a5b5

Commit message:

Code of "The Claw" without EMG;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Fri Oct 28 09:01:41 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Fri Oct 28 09:01:41 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

--- a/main.cpp	Wed Oct 19 14:03:19 2016 +0000
+++ b/main.cpp	Fri Oct 28 09:01:41 2016 +0000
@@ -1,111 +1,315 @@
 #include "mbed.h"
+#include "MODSERIAL.h"
+#define SERIAL_BAUD 115200
 #include "motordriver.h"
+#include "QEI.h"
+#include "Servo.h"
 
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
-// Serial communication using MODSERIAL
-#define SERIAL_BAUD 115200  // baud rate for serial communication
-#include "MODSERIAL.h"
+//======== Serial Communication ================================================
 MODSERIAL pc(USBTX,USBRX);
 
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
-// Timing
-const float kTimeLedToggle = 0.25f; // period with which to toggle LED
-const float kTimePrintSerial = 1.0f;// period with which data is printed
+//======== Motor and QEI =======================================================
+int Brakeable;
+int sign;
+
+// motor
+Motor Cart(D5, D4, D4, Brakeable);      // right motor
+Motor Arm(D6,D7, D7, Brakeable);        // left motor
+
+// qei
+QEI Encoder_Cart(D10, D11, NC, 64);
+QEI Encoder_Arm(D12, D13, NC, 64);
 
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
-// constants
-const int LedOn = 0;               // LED on if 0
+// servo
+Servo servo(D9);
 
-// LEDs
+//======== Miscellaneous =======================================================
+// button
+InterruptIn btn(SW2);
+InterruptIn btn2(SW3);
+
+InterruptIn btn_cart(D1);
+InterruptIn btn_arm(D2);
+InterruptIn btn_claw(D3);
+
+// led
 DigitalOut led_r(LED_RED);
 DigitalOut led_g(LED_GREEN);
 DigitalOut led_b(LED_BLUE);
 
-// ID of led that should blink; *volatile* because changed by interrupt
-volatile int part_id = 0;
+// potmeter
+AnalogIn pot_cart(A2);                  
+AnalogIn pot_arm(A3);  
 
+// ticker
+Ticker tick_part;                       // ticker to switch parts        
+
+//======== Variables ===========================================================
 // counters
-int num_turned_on_0 = 0;            // count number of times red LED turned on
-int num_turned_on_1 = 0;            // count number of times green LED turned on
-int num_turned_on_2 = 0;            // count number of times blue LED turned on
+int num_turned_on_0 = 0;                // count number of times red LED turned on
+int num_turned_on_1 = 0;                // count number of times green LED turned on
+int num_turned_on_2 = 0;                // count number of times blue LED turned on
+
+int num_claw_turned_on_0 = 0;           // count number of times red LED turned on
+int num_claw_turned_on_1 = 0;           // count number of times green LED turned on
+int num_claw_turned_on_2 = 0;           // count number of times blue LED turned on
 
+// speed
+double cart_speed = 0.5;
+double cart_stop = 0.2;
+double arm_speed = 0.3;
+double arm_stop = 0.1;
 
-//Safety for Motor
-int Brakeable; //cna the motor driver break
-int sign; //prevents throwing the motor from full foward to full reverse and stuff melting.
+// position
+float factor_cart = 0.06559;
+float factor_arm = 0.1539;
+int position_cart;
+int position_arm;
+float ain_cart;     //Variable to store the analog input of the cart
+float ain_arm;      //Variable to store the analog input of the arm
 
-//Motor
-Motor A(D6, D7, D7, Brakeable); // pwm, fwd, rev, brake                 (right)
-Motor B(D5, D4, D4, Brakeable); // pwm, fwd, rev, brake                 (left)
+// miscellaneous
+const float kTimeToggle = 0.25f;        // period with which to toggle the parts
+const int LedOn = 0;                    // LED on if 0
+volatile int part_id = 2;               // ID of what part should move, begins with the cart
+volatile int servo_id = 1;              // ID to the side the servo should move, begins in center position
 
 
 
+//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+//======== Functions and main ==============================================================
 
+// Switch between Cart, Arm and Claw
 void SwitchPart()
 {
     switch (part_id) {
+        //Cart
         case 2: {
-            // LED 0 to toggle and count:
-            led_r = LedOn;
-            if (led_r == LedOn) {
+            led_r = LedOn;          
+            if(led_r == LedOn){
                 num_turned_on_0++;
+            
+                if(btn && btn2) {                  
+                    Arm.speed(0) == 0;
+                    Cart.speed(0) == 0;                           
+                
+                }else if (btn && !btn2) {
+                    if(position_cart <= -105){                                  //If the cart is at the right side, it stops
+                    Cart.speed(-cart_stop)==-cart_stop;
+                    
+                    }else if(position_cart >= 105 && position_arm <=-60){       //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
+                    Cart.speed(cart_stop) == cart_stop;
+    
+                    }else{
+                    Cart.speed(cart_speed)==cart_speed;
+                    }
+         
+                }else if (!btn && btn2) {
+                    if(position_cart >= 105){                                   //If the cart is at the left side, it stops
+                    Cart.speed(cart_stop)==cart_stop;
+                    
+                    }else if(position_cart <= -105 && position_arm >=60){       //If the cart is at the right side and the arm is rotated 60 degrees to the right, the cart can't move to the left.
+                    Cart.speed(-cart_stop) == -cart_stop;
+                      
+                    }else{
+                    Cart.speed(-cart_speed)==-cart_speed;
+                    }
+                                            
+                }else {
+                    Arm.speed(0) == 0;
+                    Cart.speed(0) == 0; 
+                }
             }
-            
-            // LEDs to turn off:
+            // controle LED    
             led_g = not LedOn;
-            led_b = not LedOn;
-            A.speed(0) == 0;
-            B.speed(0) == 0;
+            led_b = not LedOn;     
+            
+            // encoder
+            position_cart = (Encoder_Cart.getPulses()*factor_cart) ;    
+            ain_cart = pot_cart.read();
+   
+                if (ain_cart == 0){
+                Encoder_Cart.reset();   
+                }else {}
+        
+            wait(0.1);
+            pc.baud(115200);
+            pc.printf("Distance in mm: %i\n", position_cart);
+            
             
             break;
         }
+        
+        //Arm
         case 3: {
-            // LED 1 to toggle and count:
-            led_g = LedOn;
-            A.speed(0.5) == 0.5;
-            if (led_g == LedOn) {
+            led_g = LedOn;          
+            if(led_g == LedOn){
                 num_turned_on_1++;
-            }
+            
+                if(btn && btn2) {            
+                    Arm.speed(0) == 0; 
+                    Cart.speed(0) == 0;                           
+                }
             
-            // LEDs to turn off:
+                else if (btn && !btn2) {
+                    if(position_cart > -105 && position_arm >= 30){             //If the cart is not at the end, the arm can't move any further than 30 degrees
+                    Arm.speed(-arm_stop)==-arm_stop;
+                        
+                    }else if(position_cart<= -105 && position_arm>=80){         //If the cart is at the right end, the arm can't move any further than 70 degrees
+                    Arm.speed(-arm_stop)==-arm_stop;
+    
+                    }else{
+                    Arm.speed(arm_speed)==arm_speed;
+                    } 
+         
+                }else if (!btn && btn2) {
+                    if(position_cart < 105 && position_arm <= -30){             //If the cart is not at the end, the arm can't move any further than 30 degrees 
+                    Arm.speed(arm_stop)==arm_stop;
+                    
+                    }else if(position_cart>=105 && position_arm<=-80){          //If the cart is at the left end, the arm can't move any further than 70 degrees
+                    Arm.speed(arm_stop)==arm_stop;
+    
+                    }else{
+                    Arm.speed(-arm_speed)==-arm_speed;
+                    } 
+                        
+                }else {
+                    Arm.speed(0) == 0;
+                    Cart.speed(0) == 0; 
+                }
+            } 
+            // controle LED   
             led_r = not LedOn;
-            led_b = not LedOn;
-            B.speed(0) == 0;
-                        
+            led_b = not LedOn;           
+            
+            // encoder
+            position_arm = (Encoder_Arm.getPulses()*factor_arm) ;    
+            ain_arm = pot_arm.read();
+   
+                if (ain_arm == 0){
+                Encoder_Arm.reset();   
+                }else {}
+        
+            wait(0.1);
+            pc.baud(115200);
+            pc.printf("Degrees: %i\n", position_arm);
+            
+            
             break;
         }
+        
+        //Claw
         case 4: {
-            // LED 2 to toggle and count:
             led_b = LedOn;
-            B.speed(0.5) == 0.5;
-            if (led_b == LedOn) {
+            if(led_b == LedOn){
                 num_turned_on_2++;
+                
+                if(btn && btn2){
+                       
+                }else if(btn && !btn2){
+                 servo_id ++;
+                 
+                    switch (servo_id) {
+                        case 0: {
+                            led_r = LedOn;
+                            if (led_r == LedOn) {
+                                num_claw_turned_on_0++;
+                            }
+                            
+                            led_b = not LedOn;
+                            led_g = not LedOn;
+                            
+                            servo.position(27);
+                            pc.printf("Servo position is: left \r\n");
+                            break;
+                        }
+                        case 1: {
+                            led_b = LedOn;
+                            if (led_b == LedOn) {
+                                num_claw_turned_on_1++;
+                            }
+                            
+                            led_r = not LedOn;
+                            led_g = not LedOn;
+                            
+                            servo.position(3);
+                            pc.printf("Servo position is: center \r\n");
+                            break;
+                        }
+                        case 2: {
+                            led_g = LedOn;
+                            if (led_g == LedOn) {
+                                num_claw_turned_on_2++;
+                            }
+                            
+                            led_r = not LedOn;
+                            led_b = not LedOn;
+                            
+                            servo.position(-18);
+                            pc.printf("Servo position is: right \r\n");
+                            break;
+                        }
+                    }
+                 
+                 
+                }else if(!btn && btn2){
+                servo_id --;
+                
+                switch (servo_id) {
+                    case 0: {
+                        led_r = LedOn;
+                        if (led_r == LedOn) {
+                            num_claw_turned_on_0++;
+                        }
+                        
+                        led_b = not LedOn;
+                        led_g = not LedOn;
+                        
+                        servo.position(27);
+                        pc.printf("Servo position is: left \r\n");
+                        break;
+                    }
+                    case 1: {
+                        led_b = LedOn;
+                        if (led_b == LedOn) {
+                            num_claw_turned_on_1++;
+                        }
+                        
+                        led_r = not LedOn;
+                        led_g = not LedOn;
+                        
+                        servo.position(3);
+                        pc.printf("Servo position is: center \r\n");
+                        break;
+                    }
+                    case 2: {
+                        led_g = LedOn;
+                        if (led_g == LedOn) {
+                            num_claw_turned_on_2++;
+                        }
+                        
+                        led_r = not LedOn;
+                        led_b = not LedOn;
+                        
+                        servo.position(-18);
+                        pc.printf("Servo position is: right \r\n");
+                        break;
+                    }
+                }
+                
+                }else{}       
             }
-            
-            // LEDs to turn off:
             led_r = not LedOn;
             led_g = not LedOn;
-            A.speed(0) == 0;
             
+                 
             break;
         }
     }
 }
 
-/**
- * Print the number of times each LED was turned on through serial communication
- */
-void PrintSerial()
-{
-    pc.printf("*Status*\r\n\tn_r = %d\r\n\tn_g = %d\r\n\tn_b = %d\r\n\r\n",
-        num_turned_on_0, num_turned_on_1, num_turned_on_2);
-}
 
-/**
- * Switch the led id that blinks 
- * led_blink_id goes from 0 -> 1 -> 2 -> 0 -> ...
- * @ensure led_blink_id = ++led_blink_id % kNumStates
- */
+// Switch the part
 void SetValue2() {
     part_id = 2;
    }
@@ -120,36 +324,17 @@
 }
 
 
-/**
- * Main loop.
- */
+// Main
 int main()
 {
-    // Serial comm baud rate
-    pc.baud(SERIAL_BAUD);
-    pc.printf("\r\n**RESET**\r\n");
-    
-    // Turn off all LEDs initially
     led_r = not LedOn;
     led_g = not LedOn;
     led_b = not LedOn;
     
-    // Create ticker and attach LED toggle function
-    Ticker tick_toggle_led;
-    tick_toggle_led.attach(&SwitchPart,kTimeLedToggle);
-    
-    // Create ticker and attach Print function
-    Ticker tick_print_serial;
-    tick_print_serial.attach(&PrintSerial,kTimePrintSerial);
-    
-    // Create interrupt and attach switch function
-    InterruptIn btn_cart(D2);
+    tick_part.attach(&SwitchPart,kTimeToggle); 
+   
     btn_cart.fall(&SetValue2);
-    
-    InterruptIn btn_arm(D3);
     btn_arm.fall(&SetValue3);
-
-    InterruptIn btn_claw(D4);
     btn_claw.fall(&SetValue4);
 
     while (true);