Feature/fix rpm controller draft

client/app/Adapter/In/ApiGrcp/Controller/CallController.go

@@ -6,7 +6,6 @@ import (
 	"io"
 	"log"
 	"os/exec"
-	"strconv"
 	"strings"
 	"time"
 
@@ -71,7 +70,7 @@ func (s *CallController) CallBidirectional(server call.CallService_CallBidirecti
 			valueToSend = s.ManageEngineUseCase.GetCurrentAngularSpeed()
 		}
 		sendError := server.Send(&call.CallResponse{
-			Result: strconv.Itoa(int(valueToSend)),
+			Result: fmt.Sprintf("%f",valueToSend),
 		})
 		if sendError != nil {
 			fmt.Println("finished Bidirectional")

client/app/Adapter/In/Console/Console.go

@@ -31,7 +31,7 @@ func (console *console) Run() {
 		case "1":
 			console.manageEngineUseCase.Turnaround()
 		case "2":
-			console.manageEngineUseCase.RpmControl(60.0 * 2)
+			console.manageEngineUseCase.RpmControl(120.0 * 2)
 		case "3":
 			console.manageEngineUseCase.StopRpmControl()
 		case "4":
@@ -82,19 +82,23 @@ func (console *console) setGas() {
 
 func (console *console) exitWatchdog(channel chan os.Signal) {
 	for range channel {
-		var i string
-		fmt.Println("\nDo you want to exit? (y/n)")
-		_, err := fmt.Scanf("%s", &i)
-		if err != nil {
-			fmt.Println(err)
-			console.manageEngineUseCase.TearDown()
-			panic(err)
-		}
-		if i == "y" {
-			fmt.Println("tearing down engine")
-			console.manageEngineUseCase.TearDown()
-			fmt.Println("exit succeed")
-			os.Exit(1)
-		}
+		fmt.Println("tearing down engine")
+		console.manageEngineUseCase.TearDown()
+		fmt.Println("exit succeed")
+		os.Exit(1)
+		// var i string
+		// fmt.Println("\nDo you want to exit? (y/n)")
+		// _, err := fmt.Scanf("%s", &i)
+		// if err != nil {
+		// 	fmt.Println(err)
+		// 	console.manageEngineUseCase.TearDown()
+		// 	panic(err)
+		// }
+		// if i == "y" {
+		// 	fmt.Println("tearing down engine")
+		// 	console.manageEngineUseCase.TearDown()
+		// 	fmt.Println("exit succeed")
+		// 	os.Exit(1)
+		// }
 	}
 }

client/app/Domain/Entity/ControlAlgorithm.go

@@ -1,11 +1,13 @@
 package Entity
 
+
 type ControlAlgorithm interface {
 	SetGoal(goal float64)
 	SetP(p float64)
 	SetI(i float64)
 	SetD(d float64)
 	SetSampleTime(d float64)
+	GetSampleTime() float64
 	GetIntegralTerm() float64
 	Calculate(currentValue float64) float64
 }
@@ -50,13 +52,28 @@ func (ca *controlAlgorithm) SetSampleTime(st float64) {
 	ca.sampleTime = st
 }
 
+func (ca *controlAlgorithm) GetSampleTime() float64 {
+	return ca.sampleTime
+}
+
 func (ca *controlAlgorithm) Calculate(currentValue float64) float64 {
+
 	ca.currentValue = currentValue
 	ca.currentError = ca.goal - ca.currentValue
 	proportionalTerm := ca.P * ca.currentError
-	ca.integralTerm = ca.integralTerm + ca.currentError*ca.sampleTime
+	ca.integralTerm = ca.integralTerm + ca.I*ca.currentError*ca.sampleTime
 	derivativeTerm := ca.D * (ca.currentError - ca.pastError) / ca.sampleTime
 	ca.pastError = ca.currentError
-	return proportionalTerm + ca.I*ca.integralTerm + derivativeTerm
+	total := proportionalTerm + ca.integralTerm + derivativeTerm
+
+	// fmt.Println("\tsubfuc")
+	// fmt.Println("\t\tca.goalg:", ca.goal)
+	// fmt.Println("\t\tcurrentValue:", currentValue)
+	// fmt.Println("\t\tca.currentError:", ca.currentError)
+	// fmt.Println("\t\tproportionalTerm:", proportionalTerm)
+	// fmt.Println("\t\tintegralTerm:", ca.integralTerm)
+	// // fmt.Println("\t\tderivativeTerm:",derivativeTerm)
+	// fmt.Println("\t\ttotal:", total)
+	return total
 
 }

client/app/Domain/Entity/Engine.go

@@ -137,31 +137,109 @@ func (e *engine) RpmControl(goal float64) {
 func (e *engine) contrlLoop(goal float64) {
 	e.isControlRunning <- true
 	sampleTime := time.Millisecond * 10
-	radPerSecondGoal := goal * (2 * math.Pi / 60)
+	radPerSecondGoal := 60 * (2 * math.Pi / 60)
 	e.controlAlgorithm.SetGoal(radPerSecondGoal)
-	e.controlAlgorithm.SetP(30)
-	e.controlAlgorithm.SetI(1)
+	e.controlAlgorithm.SetP(2000000)
+	e.controlAlgorithm.SetI(100)
 	e.controlAlgorithm.SetD(0)
 	e.controlAlgorithm.SetSampleTime(sampleTime.Seconds())
 
 	prevAngle := 0.0
-
 	e.brakePin.Down()
-	for range time.Tick(sampleTime) {
-		angle := e.resolution * math.Abs(float64(e.encoder.GetPosition()))
-		degreesPerSecod := (angle - prevAngle) / sampleTime.Seconds()
-		radianPerSecod := degreesPerSecod * math.Pi / 180
-		e.currentAngularSpeed = radianPerSecod
-		pidOrig := e.controlAlgorithm.Calculate(radianPerSecod)
-		pidReescalated := e.reescalePid(pidOrig)
-		e.pwmPin.SetPWM(Pin.Duty(pidReescalated), e.pwmPin.GetMaxFrequency())
-		prevAngle = angle
-		if len(e.isControlRunning) == 0 {
-			e.pwmPin.SetPWM(Pin.Duty(0), e.pwmPin.GetMaxFrequency())
-			e.brakePin.Up()
-			break
+	ticker := time.NewTicker(sampleTime)
+    done := make(chan bool)
+    go func() {
+        for {
+            select {
+            case <-done:
+                return
+            case t := <-ticker.C:
+
+
+				angle := e.resolution * math.Abs(float64(e.encoder.GetPosition()))
+
+
+				degreesPerSecod := (angle - prevAngle) / sampleTime.Seconds()
+
+				radianPerSecod := degreesPerSecod * math.Pi / 180
+
+				e.currentAngularSpeed = radianPerSecod
+				pidOrig := e.controlAlgorithm.Calculate(radianPerSecod)
+				pidReescalated := e.reescalePid(pidOrig)
+
+				e.pwmPin.SetPWM(Pin.Duty(pidReescalated), e.pwmPin.GetMaxFrequency())
+				prevAngle = angle
+				fmt.Println("Tick at", t)
+				// fmt.Println("prevAngle", prevAngle)
+				// fmt.Println("angle:", angle)
+				// fmt.Println("sampleTime.Seconds():", sampleTime.Seconds())
+				// fmt.Println("degreesPerSecod:", degreesPerSecod)
+				// fmt.Println("radianPerSecod:", radianPerSecod)
+				// fmt.Println("-------------------------------")
+				if len(e.isControlRunning) == 0 {
+					e.pwmPin.SetPWM(Pin.Duty(0), e.pwmPin.GetMaxFrequency())
+					e.brakePin.Up()
+					ticker.Stop()
+					done <- true
+					fmt.Println("Ticker stopped")
+				}
+            }
+        }
+    }()
+
+
+
+	// e.brakePin.Down()
+	// for range time.Tick(sampleTime) {
+	// 	fmt.Println("prevAngle", prevAngle)
+	// 	angle := e.resolution * math.Abs(float64(e.encoder.GetPosition()))
+	// 	fmt.Println("angle:", angle)
+	// 	fmt.Println("sampleTime.Seconds():", sampleTime.Seconds())
+	// 	degreesPerSecod := (angle - prevAngle) / sampleTime.Seconds()
+	// 	fmt.Println("degreesPerSecod:", degreesPerSecod)
+	// 	radianPerSecod := degreesPerSecod * math.Pi / 180
+	// 	fmt.Println("radianPerSecod:", radianPerSecod)
+	// 	e.currentAngularSpeed = radianPerSecod
+	// 	pidOrig := e.controlAlgorithm.Calculate(radianPerSecod)
+	// 	pidReescalated := e.reescalePid(pidOrig)
+	// 	fmt.Println("-------------------------------")
+	// 	e.pwmPin.SetPWM(Pin.Duty(math.Abs(pidReescalated)), e.pwmPin.GetMaxFrequency())
+	// 	// if pidReescalated <0{
+	// 	// 	e.Backward()
+	// 	// }else{
+	// 	// 	e.Forward()
+	// 	// }
+	// 	prevAngle = angle
+	// 	if len(e.isControlRunning) == 0 {
+	// 		e.pwmPin.SetPWM(Pin.Duty(0), e.pwmPin.GetMaxFrequency())
+	// 		e.brakePin.Up()
+	// 		break
+	// 	}
+	// 	// time.Sleep(time.Millisecond * 100)
+	// }
+}
+
+func (e *engine) reescalePid(pid float64) float64 {
+	fmt.Println("\tsubfuc:")
+	fmt.Println("\t\tpid:", pid)
+	reescalePid := pid
+	// minV := 17 * 2 * math.Pi / 60 //Probado con test que eso es a lo minimo que va
+	if math.Abs(pid) > float64(e.pwmPin.GetMaxDuty()){
+		if pid < 0{
+			fmt.Println("\t\t changed to min")
+			reescalePid = -float64(e.pwmPin.GetMaxDuty())
+		}else{
+			fmt.Println("\t\t changed to max")
+			reescalePid = float64(e.pwmPin.GetMaxDuty())
 		}
 	}
+	rangeMin:= -float64(e.pwmPin.GetMaxDuty())
+	rangeMax:=  float64(e.pwmPin.GetMaxDuty())
+	targetMin:= 0.0
+	targetMax:= float64(e.pwmPin.GetMaxDuty())
+	reescalePid = ((reescalePid-rangeMin)/(rangeMax-rangeMin))*(targetMax-targetMin) + targetMin
+	fmt.Println("\t\treescalePid:", reescalePid)
+	return reescalePid
 }
 
 func (e *engine) StopRmpControl() {
@@ -172,22 +250,6 @@ func (e *engine) StopRmpControl() {
 	}
 }
 
-func (e *engine) reescalePid(pid float64) float64 {
-	var reescalePid float64
-	if math.Abs(pid) > float64(e.pwmPin.GetMaxDuty()) {
-		reescalePid = float64(e.pwmPin.GetMaxDuty())
-		if pid < 0 {
-			reescalePid = reescalePid * -1
-		}
-	}
-	reescalePid = (reescalePid + float64(e.pwmPin.GetMaxDuty())) / 2
-	if math.Abs(pid) < float64(e.pwmPin.GetMinDuty()) {
-		reescalePid = float64(e.pwmPin.GetMinDuty())
-	}
-
-	return reescalePid
-}
-
 func (e *engine) PositionControl() {
 	//TODO implement me
 	panic("implement me")

client/client.go

@@ -6,7 +6,7 @@ func main() {
 	var app = Config.App{}
 	app.Run()
 
-	//var appEngine = Config.NewEngineApp()
-	//appEngine.Run()
-	//select {}
+	// var appEngine = Config.NewEngineApp()
+	// appEngine.Run()
+	// select {}
 }