jonas
/
ADIS_Projects
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'main' of gitlab.enterpriselab.ch:adis_team_gueti_roaster/adis_hs2022_team_4 into main

Browse Source
main
Jonas Arnold 4 years ago
parent ad76d09438 734b8859b9
commit 1d2e4e2fb7
12 changed files with 210 additions and 36 deletions
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Show Stats Download Patch File Download Diff File
  1. 96
      ADIS_Csharp/RaspiControl/Application.cs
  2. 2
      ADIS_Csharp/RaspiControl/RaspiControl.csproj
  3. 28
      ADIS_Csharp/RaspiControl/StateMachine.cs
  4. 4
      ADIS_Csharp/RobotLib/Robot.cs
  5. 37
      ADIS_Csharp/RobotLib/Status/DevStatus.cs
  6. 15
      ADIS_Csharp/RobotLib/Status/PresentEventArgs.cs
  7. 15
      ADIS_Csharp/RobotLib/Status/RoboStatus.cs
  8. 15
      ADIS_Csharp/RobotLib/Status/StatusEventArgs.cs
  9. 1
      ADIS_ESP32_Eclipse/main/robo_wrapper.c
  10. 15
      ADIS_Sumo/Sumo/Application.c
  11. 10
      ADIS_Sumo/Sumo/Pid.c
  12. 6
      ADIS_Sumo/Sumo/Turn.c

96
ADIS_Csharp/RaspiControl/Application.cs
Unescape View File

@ -1,6 +1,8 @@
using NLog.LayoutRenderers; using NLog.LayoutRenderers;
using RobotLib.Status;
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Diagnostics;
using System.Linq; using System.Linq;
using System.Text; using System.Text;
using System.Text.Json; using System.Text.Json;
@ -12,14 +14,46 @@ namespace RaspiControl {
private ChallengeFactory? challenge; private ChallengeFactory? challenge;
private StateMachine sm; private StateMachine sm;
private readonly NLog.Logger log; private readonly NLog.Logger log;
private Stopwatch stopwatch;
private uint PresentCounter;
public Application() { public Application() {
this.log = NLog.LogManager.GetCurrentClassLogger(); this.log = NLog.LogManager.GetCurrentClassLogger();
this.sm = new StateMachine(); this.sm = new StateMachine();
this.challenge = new ChallengeFactory(); this.challenge = new ChallengeFactory();
this.challenge.PublisherSubscriber.ConnectionStateChanged += this.PublisherSubscriber_ConnectionStateChanged; this.challenge.PublisherSubscriber.ConnectionStateChanged += this.PublisherSubscriber_ConnectionStateChanged;
this.challenge.PublisherSubscriber.Connect("localhost", "ADIS", "TEST"); this.challenge.PublisherSubscriber.Connect("localhost", "ADIS", "TEST");
this.challenge.RobotMobile.Status.StatusChanged += Status_StatusChanged;
this.challenge.RobotMobile.Status.PresentChanged += Status_PresentChanged;
Joystick joystick = new Joystick(); Joystick joystick = new Joystick();
joystick.JoystickChanged += Joystick_JoystickChanged; joystick.JoystickChanged += Joystick_JoystickChanged;
this.stopwatch = new Stopwatch();
this.PresentCounter = 0;
}
public void Status_StatusChanged(object? sender, StatusEventArgs e) {
try {
if (e.Status == RoboStatus.Auto) {
this.sm.MoveNext(Command.autoMode);
this.smHandler();
}
if(e.Status == RoboStatus.Final) {
this.sm.MoveNext(Command.finished);
this.smHandler();
}
if(e.Status == RoboStatus.Failure) {
this.sm.MoveNext(Command.failure);
this.smHandler();
}
}catch(Exception ex) {
this.log.Error(ex.Message);
}
}
public void Status_PresentChanged(object? sender, PresentEventArgs e) {
if(this.sm.CurrentState == ProcessState.Auto) {
this.PresentCounter++;
this.challenge.RobotStationary.SplitFlap.Display(this.PresentCounter.ToString().PadLeft(4,'0'));
}
} }
public void Run() { public void Run() {
@ -27,8 +61,12 @@ namespace RaspiControl {
if (!success) { if (!success) {
throw new Exception("Could not connect to broker on localhost"); throw new Exception("Could not connect to broker on localhost");
} else { } else {
this.sm.MoveNext(Command.Connected); try {
this.log.Info($"Current State: {this.sm.CurrentState}"); this.sm.MoveNext(Command.Connected);
this.log.Info($"Current State: {this.sm.CurrentState}");
} catch (Exception ex) {
this.log.Error(ex.Message);
}
} }
} }
@ -53,6 +91,10 @@ namespace RaspiControl {
break; break;
case ProcessState.StartMoveManual: case ProcessState.StartMoveManual:
// mode automatic:false // mode automatic:false
if (this.stopwatch.IsRunning) {
this.stopwatch.Stop();
}
this.stopwatch.Start();
this.challenge.RobotStationary.SplitFlap.Display("WALD"); this.challenge.RobotStationary.SplitFlap.Display("WALD");
this.challenge.RobotMobile.Movement.SetMobilityMode(false); this.challenge.RobotMobile.Movement.SetMobilityMode(false);
this.challenge.RobotMobile.Movement.AddSpeed(NavigationConstants.SPEED_FORWARD); this.challenge.RobotMobile.Movement.AddSpeed(NavigationConstants.SPEED_FORWARD);
@ -89,35 +131,47 @@ namespace RaspiControl {
this.challenge.RobotMobile.Movement.Stop(); this.challenge.RobotMobile.Movement.Stop();
break; break;
case ProcessState.Auto: case ProcessState.Auto:
this.challenge.RobotStationary.SplitFlap.Display("AUTO");
break; break;
case ProcessState.Final: case ProcessState.Final:
this.PresentCounter = 0;
this.stopwatch.Stop();
long elapsedTime = stopwatch.ElapsedMilliseconds / 1000;
this.challenge.RobotStationary.SplitFlap.Display(elapsedTime.ToString().PadLeft(4,'0'));
break; break;
case ProcessState.Error: case ProcessState.Error:
//this.PresentCounter = 0;
break; break;
} }
} }
private void Joystick_JoystickChanged(object sender, JoystickEventArgs e) { private void Joystick_JoystickChanged(object sender, JoystickEventArgs e) {
switch (e.Button) { try {
case JoystickButton.None: switch (e.Button) {
break; case JoystickButton.None:
case JoystickButton.Left: break;
this.sm.MoveNext(Command.JoystickLeft); case JoystickButton.Left:
break; this.sm.MoveNext(Command.JoystickLeft);
case JoystickButton.Right: break;
this.sm.MoveNext(Command.JoystickRight); case JoystickButton.Right:
break; this.sm.MoveNext(Command.JoystickRight);
case JoystickButton.Up: break;
this.sm.MoveNext(Command.JoystickUp); case JoystickButton.Up:
break; this.sm.MoveNext(Command.JoystickUp);
case JoystickButton.Down: break;
this.sm.MoveNext(Command.JoystickDown); case JoystickButton.Down:
break; this.sm.MoveNext(Command.JoystickDown);
case JoystickButton.Center: break;
this.sm.MoveNext(Command.JoystickCenter); case JoystickButton.Center:
break; this.sm.MoveNext(Command.JoystickCenter);
break;
}
if(e.Button != JoystickButton.None) {
this.smHandler();
}
} catch (Exception ex) {
this.log.Error(ex.Message);
} }
this.smHandler();
} }
} }
} }

2
ADIS_Csharp/RaspiControl/RaspiControl.csproj
Unescape View File

@ -8,7 +8,7 @@
</PropertyGroup> </PropertyGroup>
<Target Name="PostBuild" AfterTargets="PostBuildEvent"> <Target Name="PostBuild" AfterTargets="PostBuildEvent">
<Exec Command="cd &quot;$(TargetDir)&quot;&#xD;&#xA;&quot;$(SolutionDir)\SecureUpload.exe&quot; . pi-hslu:ADIS/$(ProjectName)" /> <Exec Command="cd &quot;$(TargetDir)&quot;&#xD;&#xA;&quot;$(SolutionDir)\SecureUpload.exe&quot; . pi-home:ADIS/$(ProjectName)" />
</Target> </Target>
<ItemGroup> <ItemGroup>

28
ADIS_Csharp/RaspiControl/StateMachine.cs
Unescape View File

@ -31,6 +31,8 @@ namespace RaspiControl {
JoystickRight, JoystickRight,
JoystickCenter, JoystickCenter,
autoMode, autoMode,
finished,
failure,
none none
} }
internal class StateMachine { internal class StateMachine {
@ -69,28 +71,48 @@ namespace RaspiControl {
{new StateTransition(ProcessState.Ready,Command.JoystickLeft), ProcessState.StartMoveManual }, {new StateTransition(ProcessState.Ready,Command.JoystickLeft), ProcessState.StartMoveManual },
{new StateTransition(ProcessState.Ready,Command.JoystickRight), ProcessState.StartMoveManual }, {new StateTransition(ProcessState.Ready,Command.JoystickRight), ProcessState.StartMoveManual },
{new StateTransition(ProcessState.StartMoveManual,Command.none), ProcessState.MoveManual }, {new StateTransition(ProcessState.StartMoveManual,Command.none), ProcessState.MoveManual },
{new StateTransition(ProcessState.StartMoveManual, Command.autoMode), ProcessState.Auto },
{new StateTransition(ProcessState.MoveManualF, Command.none), ProcessState.MoveManual }, {new StateTransition(ProcessState.MoveManualF, Command.none), ProcessState.MoveManual },
{new StateTransition(ProcessState.MoveManualF, Command.autoMode), ProcessState.Auto },
{new StateTransition(ProcessState.MoveManualB, Command.none), ProcessState.MoveManual }, {new StateTransition(ProcessState.MoveManualB, Command.none), ProcessState.MoveManual },
{new StateTransition(ProcessState.MoveManualB, Command.autoMode), ProcessState.Auto },
{new StateTransition(ProcessState.MoveManualL, Command.none), ProcessState.MoveManual }, {new StateTransition(ProcessState.MoveManualL, Command.none), ProcessState.MoveManual },
{new StateTransition(ProcessState.MoveManualL, Command.autoMode), ProcessState.Auto },
{new StateTransition(ProcessState.MoveManualR, Command.none), ProcessState.MoveManual }, {new StateTransition(ProcessState.MoveManualR, Command.none), ProcessState.MoveManual },
{new StateTransition(ProcessState.MoveManualR, Command.autoMode), ProcessState.Auto },
{new StateTransition(ProcessState.MoveManual, Command.JoystickUp), ProcessState.MoveManualF }, {new StateTransition(ProcessState.MoveManual, Command.JoystickUp), ProcessState.MoveManualF },
{new StateTransition(ProcessState.MoveManual, Command.JoystickDown), ProcessState.MoveManualB }, {new StateTransition(ProcessState.MoveManual, Command.JoystickDown), ProcessState.MoveManualB },
{new StateTransition(ProcessState.MoveManual, Command.JoystickLeft), ProcessState.MoveManualL }, {new StateTransition(ProcessState.MoveManual, Command.JoystickLeft), ProcessState.MoveManualL },
{new StateTransition(ProcessState.MoveManual, Command.JoystickRight), ProcessState.MoveManualR }, {new StateTransition(ProcessState.MoveManual, Command.JoystickRight), ProcessState.MoveManualR },
{new StateTransition(ProcessState.MoveManual, Command.JoystickCenter), ProcessState.StopManual }, {new StateTransition(ProcessState.MoveManual, Command.JoystickCenter), ProcessState.StopManual },
{new StateTransition(ProcessState.MoveManual, Command.autoMode), ProcessState.Auto },
{new StateTransition(ProcessState.StopManual,Command.JoystickUp), ProcessState.ResumeMoveManual }, {new StateTransition(ProcessState.StopManual,Command.JoystickUp), ProcessState.ResumeMoveManual },
{new StateTransition(ProcessState.StopManual,Command.JoystickDown), ProcessState.ResumeMoveManual }, {new StateTransition(ProcessState.StopManual,Command.JoystickDown), ProcessState.ResumeMoveManual },
{new StateTransition(ProcessState.StopManual,Command.JoystickLeft), ProcessState.ResumeMoveManual }, {new StateTransition(ProcessState.StopManual,Command.JoystickLeft), ProcessState.ResumeMoveManual },
{new StateTransition(ProcessState.StopManual,Command.JoystickRight), ProcessState.ResumeMoveManual }, {new StateTransition(ProcessState.StopManual,Command.JoystickRight), ProcessState.ResumeMoveManual },
{new StateTransition(ProcessState.StopManual, Command.autoMode), ProcessState.Auto },
{new StateTransition(ProcessState.ResumeMoveManual, Command.none),ProcessState.MoveManual }, {new StateTransition(ProcessState.ResumeMoveManual, Command.none),ProcessState.MoveManual },
{new StateTransition(ProcessState.ResumeMoveManual, Command.autoMode), ProcessState.Auto },
{new StateTransition(ProcessState.MoveManual, Command.autoMode), ProcessState.Auto },
{new StateTransition(ProcessState.Auto, Command.finished), ProcessState.Final },
{new StateTransition(ProcessState.Auto, Command.JoystickUp), ProcessState.StartMoveManual },
{new StateTransition(ProcessState.Auto, Command.JoystickDown), ProcessState.StartMoveManual },
{new StateTransition(ProcessState.Auto, Command.JoystickLeft), ProcessState.StartMoveManual },
{new StateTransition(ProcessState.Auto, Command.JoystickRight), ProcessState.StartMoveManual },
{new StateTransition(ProcessState.Auto, Command.JoystickCenter), ProcessState.StartMoveManual },
{new StateTransition(ProcessState.Auto, Command.failure), ProcessState.Error },
{new StateTransition(ProcessState.Error, Command.JoystickUp), ProcessState.StartMoveManual },
{new StateTransition(ProcessState.Error, Command.JoystickDown), ProcessState.StartMoveManual },
{new StateTransition(ProcessState.Error, Command.JoystickLeft), ProcessState.StartMoveManual },
{new StateTransition(ProcessState.Error, Command.JoystickRight), ProcessState.StartMoveManual },
{new StateTransition(ProcessState.Error, Command.JoystickCenter), ProcessState.StartMoveManual },
}; };
} }

4
ADIS_Csharp/RobotLib/Robot.cs
Unescape View File

@ -3,6 +3,7 @@ using RobotLib.SplitFlap;
using RobotLib.Movement; using RobotLib.Movement;
using RobotLib.Battery; using RobotLib.Battery;
using System.Timers; using System.Timers;
using RobotLib.Status;
namespace RobotLib namespace RobotLib
{ {
@ -23,6 +24,7 @@ namespace RobotLib
Battery = new DevBattery(Com); Battery = new DevBattery(Com);
LineSensor = new DevLineSensor(com); LineSensor = new DevLineSensor(com);
Movement = new DevMovement(com); Movement = new DevMovement(com);
Status = new DevStatus(com);
} }
else if(type == RobotMode.Stationary) else if(type == RobotMode.Stationary)
{ {
@ -46,6 +48,8 @@ namespace RobotLib
public DevMovement Movement { get; } public DevMovement Movement { get; }
public DevLineSensor LineSensor { get; } public DevLineSensor LineSensor { get; }
public DevStatus Status { get; }
public void Connect(string host, int port) public void Connect(string host, int port)
{ {
//Com.Connect(host, port); //Com.Connect(host, port);

37
ADIS_Csharp/RobotLib/Status/DevStatus.cs
Unescape View File

@ -0,0 +1,37 @@
using RobotLib.Communication;
using RobotLib.SplitFlap;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Text.Json;
using System.Threading.Tasks;
namespace RobotLib.Status {
public class DevStatus: DevBase {
private const string TOPIC_STATUS_MODE = "/mobile/status/mode/";
private const string TOPIC_STATUS_PRESENT = "/mobile/status/present/";
public event EventHandler<StatusEventArgs> StatusChanged;
public event EventHandler<PresentEventArgs> PresentChanged;
public DevStatus(IPublisherSubscriber com) : base(com, new List<string>() { TOPIC_STATUS_MODE, TOPIC_STATUS_PRESENT }) { }
protected override void ParseMessage(string fromTopic, string message) {
if (fromTopic == TOPIC_STATUS_MODE) {
StatusEventArgs eventArgs = new(message);
StatusChanged?.Invoke(this, eventArgs);
}else if(fromTopic == TOPIC_STATUS_PRESENT) {
PresentEventArgs presentEventArgs = new(true);
PresentChanged?.Invoke(this, presentEventArgs);
}
}
private T GetValueFromMesage<T>(string parameter, string message) {
var data = JsonSerializer.Deserialize<Dictionary<string, T>>(message);
data.TryGetValue(parameter, out T value);
return value;
}
}
}

15
ADIS_Csharp/RobotLib/Status/PresentEventArgs.cs
Unescape View File

@ -0,0 +1,15 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace RobotLib.Status {
public class PresentEventArgs {
public bool Present { get; }
public PresentEventArgs(bool present) {
Present = present;
}
}
}

15
ADIS_Csharp/RobotLib/Status/RoboStatus.cs
Unescape View File

@ -0,0 +1,15 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace RobotLib.Status {
public static class RoboStatus {
public static readonly string Auto = "AUTO";
public static readonly string Manual = "MANUAL";
public static readonly string Final = "FINAL";
public static readonly string Failure = "FAILURE";
}
}

15
ADIS_Csharp/RobotLib/Status/StatusEventArgs.cs
Unescape View File

@ -0,0 +1,15 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace RobotLib.Status {
public class StatusEventArgs {
public string Status { get; }
public StatusEventArgs(string status) {
Status = status;
}
}
}

1
ADIS_ESP32_Eclipse/main/robo_wrapper.c
Unescape View File

@ -31,6 +31,7 @@ bool Robo_Wrapper_SetMode(bool automatic){
unsigned char cmd[BUF_SIZE] = RS_CMD_DRIVE_PREFIX; unsigned char cmd[BUF_SIZE] = RS_CMD_DRIVE_PREFIX;
McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)"mode speed"); McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)"mode speed");
SHELL_SendToRobotAndGetResponse(cmd, response, sizeof(response)); SHELL_SendToRobotAndGetResponse(cmd, response, sizeof(response));
SHELL_SendToRobotAndGetResponse((const unsigned char*)"app manualdrive", response, sizeof(response));
McuShell_SendStr(response, McuShell_GetStdio()->stdOut); McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
} }
return true; return true;

15
ADIS_Sumo/Sumo/Application.c
Unescape View File

@ -144,7 +144,9 @@
#if PL_HAS_MIDI #if PL_HAS_MIDI
#include "MidiMusic.h" #include "MidiMusic.h"
#endif #endif
#if PL_CONFIG_USE_ESP32
#include "McuESP32.h"
#endif
typedef enum { typedef enum {
APP_STATE_STARTUP, APP_STATE_STARTUP,
APP_STATE_INIT, APP_STATE_INIT,
@ -371,23 +373,28 @@ static void StateMachine(bool buttonPress) {
presentCnt = MAZE_GetPresentCount(); presentCnt = MAZE_GetPresentCount();
MAZE_ClearSolution(); MAZE_ClearSolution();
LF_StartFollowing(); LF_StartFollowing();
McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"AUTO\"", McuESP32_GetTxToESPStdio()->stdOut);
McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
} }
break; break;
case APP_STATE_AUTO: case APP_STATE_AUTO:
if(!LF_IsFollowing() && MAZE_IsSolved()){ if(!LF_IsFollowing() && MAZE_IsSolved()){
//done //done
SHELL_SendString((unsigned char*)"MAZE: done, stopped!!!\r\n"); SHELL_SendString((unsigned char*)"MAZE: done, stopped!!!\r\n");
McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"FINAL\"", McuESP32_GetTxToESPStdio()->stdOut);
McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
appState = APP_STATE_IDLE; appState = APP_STATE_IDLE;
}else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure < 5){ }else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure < 5){
// failed // failed
mazeFailure++; mazeFailure++;
SHELL_SendString((unsigned char*)"MAZE: Failure, stopped OMG!!!\r\n");
MAZE_ClearSolution(); MAZE_ClearSolution();
DRV_SetSpeed(0, 0); DRV_SetSpeed(0, 0);
DRV_SetMode(DRV_MODE_SPEED); DRV_SetMode(DRV_MODE_SPEED);
LF_StartFollowing(); LF_StartFollowing();
}else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure >= 5){ }else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure >= 5){
appState = APP_STATE_IDLE; appState = APP_STATE_IDLE;
McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"FAILURE\"", McuESP32_GetTxToESPStdio()->stdOut);
McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
} }
if(MAZE_GetPresentCount() != presentCnt){ if(MAZE_GetPresentCount() != presentCnt){
presentCnt = MAZE_GetPresentCount(); presentCnt = MAZE_GetPresentCount();
@ -396,6 +403,8 @@ static void StateMachine(bool buttonPress) {
McuUtility_Num8uToStr(counter, sizeof(counter), presentCnt); McuUtility_Num8uToStr(counter, sizeof(counter), presentCnt);
SHELL_SendString((unsigned char*)counter); SHELL_SendString((unsigned char*)counter);
SHELL_SendString((unsigned char*)"\r\n"); SHELL_SendString((unsigned char*)"\r\n");
McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/present/\" \"true\"", McuESP32_GetTxToESPStdio()->stdOut);
McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
} }
break; break;
default: default:
@ -499,6 +508,8 @@ static uint8_t AutoCalibrateReflectance(void) {
static uint8_t SetManualMode(){ static uint8_t SetManualMode(){
appState = APP_STATE_MANUAL_MOVE; appState = APP_STATE_MANUAL_MOVE;
McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"MANUAL\"", McuESP32_GetTxToESPStdio()->stdOut);
McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
return ERR_OK; return ERR_OK;
} }

10
ADIS_Sumo/Sumo/Pid.c
Unescape View File

@ -630,11 +630,11 @@ void PID_Init(void) {
lineFwConfig.lastError = 0; lineFwConfig.lastError = 0;
lineFwConfig.integral = 0; lineFwConfig.integral = 0;
#elif PL_IS_INTRO_ZUMO_ROBOT2 || PL_IS_INTRO_ZUMO_K22 #elif PL_IS_INTRO_ZUMO_ROBOT2 || PL_IS_INTRO_ZUMO_K22
lineFwConfig.pFactor100 = 5500; lineFwConfig.pFactor100 = 4000;//5500;
lineFwConfig.iFactor100 = 15; lineFwConfig.iFactor100 = 15;
lineFwConfig.dFactor100 = 100; lineFwConfig.dFactor100 = 500;//100;
lineFwConfig.iAntiWindup = 100000; lineFwConfig.iAntiWindup = 100000;
lineFwConfig.maxSpeedPercent = 40; lineFwConfig.maxSpeedPercent = 25;//40;
lineFwConfig.lastError = 0; lineFwConfig.lastError = 0;
lineFwConfig.integral = 0; lineFwConfig.integral = 0;
#else #else
@ -672,11 +672,11 @@ void PID_Init(void) {
posLeftConfig.iAntiWindup = 200; posLeftConfig.iAntiWindup = 200;
posLeftConfig.maxSpeedPercent = 40; posLeftConfig.maxSpeedPercent = 40;
#elif (PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE) /* line/maze, high speed */ #elif (PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE) /* line/maze, high speed */
posLeftConfig.pFactor100 = 1000; posLeftConfig.pFactor100 = 2000;//1000;
posLeftConfig.iFactor100 = 2; posLeftConfig.iFactor100 = 2;
posLeftConfig.dFactor100 = 50; posLeftConfig.dFactor100 = 50;
posLeftConfig.iAntiWindup = 200; posLeftConfig.iAntiWindup = 200;
posLeftConfig.maxSpeedPercent = 40; posLeftConfig.maxSpeedPercent = 35;//40;
#else /* defaults */ #else /* defaults */
posLeftConfig.pFactor100 = 1000; posLeftConfig.pFactor100 = 1000;
posLeftConfig.iFactor100 = 1; posLeftConfig.iFactor100 = 1;

6
ADIS_Sumo/Sumo/Turn.c
Unescape View File

@ -63,11 +63,11 @@
#define TURN_STEPS_POST_LINE_TIMEOUT_MS 500 #define TURN_STEPS_POST_LINE_TIMEOUT_MS 500
#define TURN_STEPS_STOP_TIMEOUT_MS 150 #define TURN_STEPS_STOP_TIMEOUT_MS 150
#elif PL_IS_INTRO_ZUMO_K22 /* no LiPo */ #elif PL_IS_INTRO_ZUMO_K22 /* no LiPo */
#define TURN_STEPS_90 720 #define TURN_STEPS_90 650//720
/*!< number of steps for a 90 degree turn */ /*!< number of steps for a 90 degree turn */
#define TURN_STEPS_LINE 100 /*230*/ #define TURN_STEPS_LINE 170//100 /*230*/
/*!< number of steps stepping over the line */ /*!< number of steps stepping over the line */
#define TURN_STEPS_POST_LINE 150 #define TURN_STEPS_POST_LINE 240//150
/*!< number of steps after the line, before making a turn */ /*!< number of steps after the line, before making a turn */
#define TURN_STEPS_90_TIMEOUT_MS 1000 #define TURN_STEPS_90_TIMEOUT_MS 1000
#define TURN_STEPS_LINE_TIMEOUT_MS 200 #define TURN_STEPS_LINE_TIMEOUT_MS 200

Write Preview
Loading…
Cancel
Save