TreppenRobo
first commit
Dependencies: PM2_Libary
Diff: main.cpp
- Branch:
- lupo
- Revision:
- 41:a8557360c15e
- Parent:
- 40:04b032b01dd5
- Child:
- 42:ec3a88a24666
--- a/main.cpp Wed May 18 13:50:44 2022 +0200
+++ b/main.cpp Thu May 19 12:25:41 2022 +0200
@@ -5,7 +5,7 @@
#include "math.h"
//*******************************************************************************************************************************************************************
// Defined Variables in mm coming from Hardware-team. Need to be updated
-const float wheel_diameter = 30; // diameter of wheel with caterpillar to calculate mm per wheel turn (4)
+const float wheel_diameter = 30.0f; // diameter of wheel with caterpillar to calculate mm per wheel turn (4)
const float arm_length = 118.5; // lenght of arm from pivotpoint to pivotpoint (3)
const float dist_arm_attach_distsensor = 20; // distance between pivot point arm on body to start distancesensor on top in horizontal (6)
const float dist_distsensors = 200; // distance between the two distancesensors on top of Wall-E (9)
@@ -55,12 +55,12 @@
// Hardware controll Setup and functions (motors and sensors)
//these variables represent relative position NOT absolut
-float startPos = -0.525; //from last lift up position to start position
+float startPos = -0.555; //from last lift up position to start position
float liftPos = -0.555; //from start position to lift up position
// create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
const float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
-const float counts_per_turn_wheels = 20.0f * 78.125f; // define counts per turn at gearbox end (counts/turn * gearratio) for wheels
+const float counts_per_turn_wheels = 20.0f * 78.125f;// define counts per turn at gearbox end (counts/turn * gearratio) for wheels
const float counts_per_turn_arm = 20.0f * 78.125f * 19.0f; // define counts per turn at gearbox end (counts/turn * gearratio) for arm
const float kn = 180.0f / 12.0f; // define motor constant in rpm per V
const float k_gear = 100.0f / 78.125f; // define additional ratio in case you are using a dc motor with a different gear box, e.g. 100:1 (DC with 100:1 has 2'000 turns for 360°)
@@ -79,8 +79,8 @@
//placeholder variables for prototype testing
-const int drive_straight_mm = 2; // placeholder for testing drives amount forward
-const int drive_back_mm = -2; // placeholder for testing drives amount backwards
+const float drive_straight_mm = 250.0; // placeholder for testing drives amount forward
+const float drive_back_mm = -20.0f; // placeholder for testing drives amount backwards
int ToNextFunction = 0; // current state of the system (which function is beeing executed)
int state=0;
float desired_pos;
@@ -108,9 +108,9 @@
//calculates the deg which the wheels have to turn in order to cover specified distance in mm
//PARAM: distance = distance to drive in milimeter
//RETURN: deg_wheel = degree which the motor has to turn in order to cover distance(mm)
-float wheel_dist_to_deg(int distance)
+float wheel_dist_to_deg(float distance)
{
- float deg_wheel = distance / (wheel_diameter * pi) * 360;
+ float deg_wheel = (distance* 360.0f) / (wheel_diameter * pi) ;
return deg_wheel;
}
@@ -178,7 +178,7 @@
if(distance == 0){
return 10; //sensor value is outside the expected range
}
- if((distance <= (setpoint + 5)) && (distance >= (setpoint - 5))){
+ if((distance <= (setpoint + 2)) && (distance >= (setpoint - 2))){
return 3; //the distance to the next step is in ±1cm of the setpoint
}
if(distance < setpoint){
@@ -212,21 +212,25 @@
{
float diff;
- if (desired_pos==0) {
- desired_pos=turn_relative_deg(startPos, positionController_M_Arm.getRotation());
- }
-
-
- positionController_M_Arm.setDesiredRotation(desired_pos, max_speed_rps_arm); // command to turn motor to desired deg.
-
-
+ int gripper=StepDetection_down(ir_analog_in_Distance_L);
+ if (desired_pos==0) {
+ desired_pos=turn_relative_deg(startPos, positionController_M_Arm.getRotation());
+ positionController_M_Arm.setDesiredRotation(desired_pos, max_speed_rps_arm); // command to turn motor to desired deg.
+ }
diff =abs( desired_pos-(positionController_M_Arm.getRotation()));
printf("Set arm Position ARM (rot): %3.3f Desired:%3.3f State:%d ToNextfunction:%d Diff:%3.3f\n",
positionController_M_Arm.getRotation(), desired_pos, state, ToNextFunction, diff);
- if (diff<=0.009){
+
+
+ if (gripper){
+ desired_pos=turn_relative_deg(0.0, positionController_M_Arm.getRotation() );
+ positionController_M_Arm.setDesiredRotation(desired_pos, max_speed_rps_arm);
+ }
+
+ if((diff<0.009)&&gripper){
return 1;
}
else {
@@ -242,7 +246,7 @@
float diff_L;
if (desired_pos==0) {
- desired_pos= wheel_dist_to_deg(distance);
+ desired_pos= distance/(wheel_diameter*pi);
float relativ_turns_rightmotor = turn_relative_deg(desired_pos, positionController_M_right.getRotation());
float relativ_turns_leftmotor = turn_relative_deg(desired_pos, positionController_M_left.getRotation());
}
@@ -253,8 +257,8 @@
diff_R= abs(desired_pos-(positionController_M_right.getRotation()));
diff_L= abs(desired_pos-(positionController_M_left.getRotation()));
- // printf("Drive Straight Position Right(rot): %3.3f; Position Left (rot): %3.3f Desired: %3.3f Diff:%3.3f State:%d ToNextfunction:%d\n",
-// positionController_M_right.getRotation(),positionController_M_left.getRotation(),desired_pos,diff_L, state, ToNextFunction);
+ printf("Drive Straight Position Right(rot): %3.3f; Position Left (rot): %3.3f Desired: %3.3f Diff:%3.3f State:%d ToNextfunction:%d\n",
+ positionController_M_right.getRotation(),positionController_M_left.getRotation(),desired_pos,diff_L, state, ToNextFunction);
if ((diff_R<=0.03) && (diff_L<=0.03))
{
return 1;
@@ -301,7 +305,7 @@
user_button.rise(&user_button_released_fcn);
mechanical_button.mode(PullDown);
printf("test");
-
+ ToNextFunction = 0;
while (true)
{
@@ -312,11 +316,11 @@
{
enable_motors=1;
}
-
-
+
+
switch (ToNextFunction)
{
-
+
case 0: while (mechanical_button.read()!=1)
{
positionController_M_Arm.setDesiredRotation(-1,0.5);
@@ -358,14 +362,14 @@
}
break;
- case 4: state=nextStepDetection(ir_distance_mm_L,10);
+ case 4: state=nextStepDetection(ir_distance_mm_L,4);
if (state==3){
nextStep=1;
}
- printf("distance:%3.3f Output:%d\n NextStep:%d", ir_distance_mm_L, nextStepDetection(ir_distance_mm_L,10), nextStep);
+ printf("distance:%3.3f Output:%d NextStep:%d\n ", ir_distance_mm_L, nextStepDetection(ir_distance_mm_L,4), nextStep);
ToNextFunction +=1;
state=0;