first commit
Dependencies: PM2_Libary
Diff: main.cpp
- Revision:
- 34:9f779e91168e
- Parent:
- 33:70ea029a69e8
- Child:
- 35:96ed18b1af94
diff -r 70ea029a69e8 -r 9f779e91168e main.cpp --- a/main.cpp Wed Apr 06 12:38:20 2022 +0200 +++ b/main.cpp Sun Apr 10 19:40:59 2022 +0200 @@ -12,7 +12,7 @@ void user_button_released_fcn(); // while loop gets executed every main_task_period_ms milliseconds -int main_task_period_ms = 50; // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second +int main_task_period_ms = 30; // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second Timer main_task_timer; // create Timer object which we use to run the main task every main task period time in ms // Sharp GP2Y0A41SK0F, 4-40 cm IR Sensor @@ -26,8 +26,8 @@ float pwm_period_s = 0.00005f; // define pwm period time in seconds and create FastPWM objects to command dc motors //motor pin declaration -FastPWM pwm_M_right(PA_9); -FastPWM pwm_M_left(PB_13); +FastPWM pwm_M_right(PB_13); +FastPWM pwm_M_left(PA_9); FastPWM pwm_M_arm(PA_10); //Encoder pin declaration @@ -38,7 +38,7 @@ // create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack float counts_per_turn_wheels = 2000.0f * 100.0f; // define counts per turn at gearbox end (counts/turn * gearratio) for wheels -float counts_per_turn_arm = 40000.0f * 100.0f; // define counts per turn at gearbox end (counts/turn * gearratio) for arm +float counts_per_turn_arm = 2000.0f * 100.0f; // define counts per turn at gearbox end (counts/turn * gearratio) for arm float kn = 180.0f / 12.0f; // define motor constant in rpm per V 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 float kp = 0.1f; // define custom kp, this is the default speed controller gain for gear box 78.125:1 @@ -58,36 +58,38 @@ // LSM9DS1 imu(PC_9, PA_8); // create LSM9DS1 comunication object, if you want to be able to use the imu you need to #include "LSM9DS1_i2c.h" //Platzhalter Variabeln für die Positionierung -int16_t PositionStair = 20; -int16_t PositionBackOff = 100; -int16_t degArmStart = 40; -int16_t degArmLift = 18; -int ToNextFunction = 1; +float PositionStair = 0.2; +float PositionBackOff = -0.5; +float degArmStart = 0.5; +float degArmLift = -0.5; +int ToNextFunction = 0; +float max_speed_rps = 0.5f; -int StartPosition(int16_t deg){ +int StartPosition(float deg){ positionController_M_Arm.setDesiredRotation(deg); return NULL; } //Drives forward into the next step -int Drive(int16_t dist){ - int8_t i = 0; //prov condition variable +int Drive(float dist){ + +float distance; - int8_t distance = dist; //distance which will be driven in [mm] - float factor = 1.0f; //factor for calculating the value in to the float which will be given to the setDesiredRotation function - float distanceValue = float(distance)*factor; +distance=dist; + - positionController_M_right.setDesiredRotation(distanceValue); - positionController_M_left.setDesiredRotation(distanceValue); + positionController_M_right.setDesiredRotation(distance,max_speed_rps); + positionController_M_left.setDesiredRotation(distance,max_speed_rps); + return 0; } //only turns the arm until the robot is on the next step //not yet clear if the motor controler function drives to a absolute poition or if it drives the given distance relative to the current position -int LiftUp(int16_t deg){ - int8_t rotation = deg; +int LiftUp(float deg){ + int8_t i = 0; //prov condition variable positionController_M_Arm.setDesiredRotation(deg); @@ -133,40 +135,45 @@ { // attach button fall and rise functions to user button object user_button.fall(&user_button_pressed_fcn); - user_button.rise(&user_button_released_fcn); +user_button.rise(&user_button_released_fcn); while (true){ + enable_motors = 1; + ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f; - printf("test pow function 2 ^ 2 %lf\n",powerx(2,2)); - printf("test mapping function %f\n", mapping(ir_distance_mV)); + // printf("test pow function 2 ^ 2 %lf\n",powerx(2,2)); + //printf("test mapping function %f\n", mapping(ir_distance_mV)); - printf("IR sensor (mV): %3.3f\n", ir_distance_mV); + //printf("IR sensor (mV): %3.3f\n", ir_distance_mV); switch (ToNextFunction) { case 1: StartPosition(degArmStart); - printf("a"); + printf("Case 1: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation()); // ToNextFunction+=1; break; case 2: Drive(PositionStair); - printf("b"); + printf("Case 2: Position Right(rot): %3.3f; Position Left (rot): %3.3f\n", + positionController_M_right.getRotation(),positionController_M_left.getRotation()); // ToNextFunction+=1; break; - case 3:// LiftUp(degArmLift); + case 3: LiftUp(degArmLift); // ToNextFunction+=1; - printf("c"); + printf("Case 3: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation()); break; case 4: Drive(PositionBackOff); - printf("d"); + printf("Case 4: Position Right(rot): %3.3f; Position Left (rot): %3.3f\n", + positionController_M_right.getRotation(),positionController_M_left.getRotation()); // ToNextFunction+=1; break; case 5: LiftUp(degArmStart); - printf("d"); + printf("Case 5: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation()); // ToNextFunction = 0; break; + default: ; }