Merge pull request #203 from CPSSD/feature/Localization

Feature/localization

Author: DarraghGriffin

source/edison/src/RobotDriverProtocol/RobotDriverProtocol.go

@@ -130,6 +130,8 @@ func processCompassResponse(responseBuffer []uint8) {
 	compassResponse.ID = (uint16(responseBuffer[0]) << 8) + uint16(responseBuffer[1])
 	compassResponse.Angle = (uint16(responseBuffer[2]) << 8) + uint16(responseBuffer[3])
 	compassResponse.Success = (responseBuffer[4] > 0)
+	fmt.Println("Passing compassResponse to responseHandler")	
+	fireResponseHandler(compassResponse)
 }
 
 // processResponse will process a response
@@ -158,7 +160,7 @@ func sendNextCommand() bool {
 
 	data := make([]uint8, 1)
 	data[0] = length
-	time.Sleep(time.Millisecond/10)
+	time.Sleep(time.Millisecond / 10)
 	SPI.TransferAndReceiveData(data)
 	if data[0] > length {
 		length = data[0]
@@ -173,10 +175,10 @@ func sendNextCommand() bool {
 			buffersToSend = buffersToSend[1:]
 		}
 
-		time.Sleep(time.Millisecond/10)
+		time.Sleep(time.Millisecond / 10)
 
 		for i := 0; i < len(dataBuffer); i++ {
-			time.Sleep(time.Millisecond/10)
+			time.Sleep(time.Millisecond / 10)
 			data[0] = dataBuffer[i]
 			SPI.TransferAndReceiveData(data)
 			dataBuffer[i] = data[0]
@@ -193,7 +195,7 @@ func sendNextCommand() bool {
 			notFinished = true
 		}
 
-		time.Sleep(time.Millisecond/10)
+		time.Sleep(time.Millisecond / 10)
 		SPI.TransferAndReceiveData(data)
 
 		return (notFinished == true || data[0] == 1)

source/edison/src/mapping/maps/RDPIntegration.go

@@ -17,9 +17,6 @@ func RDPInit() {
 
 // RDPConnector handles the incoming data.
 func RDPConnector(data interface{}) {
-	//	fmt.Println()
-	//	fmt.Println("\tData Recieved =>", data)
-
 	switch response := data.(type) {
 	case RobotDriverProtocol.MoveResponse:
 		moveResponse(response)
@@ -29,12 +26,26 @@ func RDPConnector(data interface{}) {
 		scanResponse(response)
 	case RobotDriverProtocol.StopResponse:
 		stopResponse(response)
+	case RobotDriverProtocol.CompassResponse:
+		compassResponse(response)
 	}
 }
 
+func compassResponse(response RobotDriverProtocol.CompassResponse) {
+	fmt.Println("[Compass Response] Angle: ", response.Angle)
+	UpdateCompassHeading(int(response.Angle))
+}
+
 func moveResponse(response RobotDriverProtocol.MoveResponse) {
 	fmt.Print("[Move Response] Angle:", response.Angle, " // Magnitude:", response.Magnitude)
-	//	fmt.Println("\t[Response] { ID:", response.ID, " // Type:", response.Type, "}")
+
+	if !response.Success {
+		fmt.Println("Problem moving... Doing a scan.");
+		followingPath = false
+		RobotMap.MoveRobotAlongLine(float64(response.Angle), float64(response.Magnitude))
+		RobotDriverProtocol.Scan()
+		return
+	}
 
 	if followingPath {
 		lastX := RobotMap.GetRobot().GetX()
@@ -54,7 +65,6 @@ func moveResponse(response RobotDriverProtocol.MoveResponse) {
 
 func scanResponse(response RobotDriverProtocol.ScanResponse) {
 	fmt.Println("[Scan Response] Degree: ", response.Degree, " // Distance: ", response.Distance)
-	//	fmt.Println("\t[Response] { ID:", response.ID, " // Type:", response.Type, "}")
 
 	// Add a wall at the specific location.
 	// When last response, find next location to move to in map.go
@@ -63,20 +73,26 @@ func scanResponse(response RobotDriverProtocol.ScanResponse) {
 	if response.Last {
 		if firstScan {
 			firstScan = false
-			RobotMap.addBufferToMap()
+			if CurrentlyMapping {
+				RobotMap.addBufferToMap()
+			}
 		} else {
 			fmt.Println("Robot should be here at this location: (", RobotMap.GetRobot().GetX(), ", ", RobotMap.GetRobot().GetY(), ")")
-			x, y, rotation := RobotMap.FindLocation()
+			x, y, rotation := RobotMap.FindLocation(200 / BitmapScale, 20, 1)
 			RobotMap.GetRobot().MoveToPoint(x, y, true)
 			RobotMap.GetRobot().Rotate(float64(rotation))
-			RobotMap.addBufferToMap()
+			if CurrentlyMapping {
+				RobotMap.addBufferToMap()
+			}
 		}
 
 		lastAction = "Scan"
 		currentID = -1
 
 		RobotMap.Print(nil)
-		RobotMap.ContinueToNextArea()
+		if CurrentlyMapping {
+			RobotMap.ContinueToNextArea()
+		}
 	}
 }
 
@@ -99,7 +115,7 @@ func stopResponse(response RobotDriverProtocol.StopResponse) {
 	RobotMap.MoveRobotAlongLine(float64(response.Angle), float64(response.Magnitude))
 	lastAction = "Stop"
 	currentID = -1
-
+	followingPath = false
 	RobotDriverProtocol.Scan()
 
 }

source/edison/src/mapping/maps/line.go

@@ -14,6 +14,14 @@ type Line struct {
 	Slope    int
 }
 
+func (this Line) split() (Line, Line) {
+	midPoint := Point{(this.P1.X + this.P2.X) / 2, (this.P1.Y + this.P2.Y) / 2}
+	line1, line2 := Line{P1: this.P1, P2: midPoint}, Line{P1: midPoint, P2: this.P2}
+	line1.Slope, line1.Vertical = line1.getSlope()
+	line2.Slope, line2.Vertical = line2.getSlope()
+	return line1, line2
+}
+
 // Gets the slope of the line. Returns (slope int, vertical bool). If vertical, Slope will be set to 0.
 func (this Line) getSlope() (int, bool) {
 	if this.P1.X == this.P2.X {

source/edison/src/mapping/maps/map.go

@@ -18,12 +18,20 @@ const Debug = false
 var BitmapScale int
 var scanBuffer []RobotDriverProtocol.ScanResponse
 var finishedMapping = false
+var CurrentlyMapping = true
 
 // RobotMap is the current environment being mapped
 var firstScan = true
 var checkLocation = false
 var followingPath = false
 
+var compassOffset = -1
+var compassHeading = 0
+var waitingForHeading = false
+
+var distanceSinceLastScan = 0
+var minimumScanDistance = 50
+
 var pathIndex = 0
 
 // RobotMap is a map of the current room being mapped
@@ -57,10 +65,6 @@ func MapInit(bitmapScale int) {
 	fmt.Println("[RDP Link Ready]")
 
 	scanBuffer = make([]RobotDriverProtocol.ScanResponse, 0)
-
-	defer RobotMap.Print(nil)
-
-	RobotDriverProtocol.Scan()
 }
 
 // GetSeenMap returns the seen map.
@@ -78,11 +82,23 @@ func (this *Map) GetRobot() *Robot {
 	return &this.robot
 }
 
+func UpdateCompassHeading(angle int) {
+	if compassOffset == -1 {
+		compassOffset = angle
+	}
+	compassHeading = (angle - compassOffset + 360) % 360
+	waitingForHeading = false
+}
+
 // FollowingPath Returns the followingPath boolean
 func FollowingPath() bool {
 	return followingPath
 }
 
+func StopFollowingPath() {
+	followingPath = false
+}
+
 // CreateMap creates an empty 1x1 map (this single point will be the robots starting position.)
 func CreateMap() (createdMap Map) {
 	createdMap = Map{width: 1, height: 1, robot: Robot{0, 0, 0}}
@@ -161,16 +177,14 @@ func (this *Map) addBufferToMap() {
 }
 
 // FindLocation finds and returns the location of the robot based on the current scan information and the previous map information
-func (this *Map) FindLocation() (int, int, float64) {
-	rotationAmmount := 30
-	rotationJump := 1
+func (this *Map) FindLocation(areaToSearch, rotationAmmount, rotationJump int) (int, int, float64) {
 	x, y := int(this.GetRobot().GetX()), int(this.GetRobot().GetY())
 	var count = -999
 	var angle = int(this.GetRobot().GetRotation())
 	var currentRotation = int(this.GetRobot().GetRotation())
 	for i := -rotationAmmount; i <= rotationAmmount; i += rotationJump {
 		currentRotation = i + int(this.GetRobot().GetRotation())
-		tempX, tempY, tempCount := this.findLocation(createMapFragment(scanBuffer, currentRotation))
+		tempX, tempY, tempCount := this.findLocation(areaToSearch, createMapFragment(scanBuffer, currentRotation))
 		if tempCount > count {
 			x = tempX
 			y = tempY
@@ -182,14 +196,22 @@ func (this *Map) FindLocation() (int, int, float64) {
 	return x, y, float64(angle)
 }
 
-func (this *Map) findLocation(fragment Map) (int, int, int) {
+func (this *Map) findLocation(areaToSearch int, fragment Map) (int, int, int) {
 	fmt.Println("Attempting to find location...")
 	mX, mY, mCount := int(this.GetRobot().GetX()), int(this.GetRobot().GetY()), 0
 
 	fmt.Println("Robots Assumed Location: (", mX, ",", mY, ")")
 
-	for i := 0; i < this.width; i++ {
-		for j := 0; j < this.height; j++ {
+	xSearch := int(this.GetRobot().GetX())
+	ySearch := int(this.GetRobot().GetY())
+
+	if areaToSearch == 0 {
+		xSearch = this.width
+		ySearch = this.height
+	}
+
+	for i := xSearch - areaToSearch; i < xSearch + areaToSearch; i++ {
+		for j := ySearch - areaToSearch; j < ySearch + areaToSearch; j++ {
 			if i >= 0 && j >= 0 && i < this.width && j < this.height {
 				count, _, _ := this.probabilityAtLocation(fragment, int(i), int(j))
 				if count != 0 {
@@ -230,9 +252,20 @@ func (this *Map) probabilityAtLocation(fragment Map, x int, y int) (int, int, in
 }
 
 func (this *Map) TakeNextStep(lastX int, lastY int) {
+	// If we haven't scanned in a while: do a scan and return.
+	if distanceSinceLastScan > minimumScanDistance {
+		distanceSinceLastScan = 0
+		RobotDriverProtocol.Scan()
+		return
+	}
+
+	// Otherwise just continue moving
 	if len(path) != 0 {
+		followingPath = true
 		robotPoint := Point{int(this.GetRobot().GetX()), int(this.GetRobot().GetY())}
 		line, movesLeft := this.getNextMove(int(this.GetRobot().GetX()), int(this.GetRobot().GetY()), lastX, lastY, path)
+		distanceSinceLastScan += line.getLength()
+		fmt.Println("Current length of line: ", line.getLength())
 
 		fmt.Println("currentLocation: ", robotPoint)
 		// If you are 1 move away from end point
@@ -251,7 +284,8 @@ func (this *Map) TakeNextStep(lastX int, lastY int) {
 
 		degree := line.getAngleFrom(robotPoint)
 		magnitude := line.getLength()
-
+		offset := RobotMap.GetRobot().GetRotation()
+		degree = (degree + int(offset)) % 360
 		fmt.Println("[TakeNextStep]: Required Move: ", degree, " -> ", magnitude)
 
 		RobotDriverProtocol.Move(uint16(degree), uint32(magnitude))
@@ -269,6 +303,18 @@ func (this *Map) TakeNextStep(lastX int, lastY int) {
 // Used when following a path into unseen areas to prevent crashes.
 func (this *Map) MoveRobotAlongPath(newPath [][]bool, stopBeforePoint bool) {
 	path = newPath
+	lines := BitmapToVector(path)
+
+	for i := 0; i < len(lines); i++ {
+		if lines[i].getLength() > minimumScanDistance {
+			line1, line2 := lines[i].split()
+			lines[i] = line1
+			lines = append(lines, line2)
+			i--
+		}
+	}
+
+	lineMap = lines
 	followingPath = true
 	this.TakeNextStep(-1, -1)
 	fmt.Println("Queued a path for movement...")
@@ -286,18 +332,18 @@ func (this *Map) MoveRobotToPoint(x, y int) {
 
 // Returns the next move in the path.
 func (this *Map) getNextMove(x, y, prevX, prevY int, path [][]bool) (line Line, possibleMove bool) {
-	lines := BitmapToVector(path)
-	fmt.Println("Line Map: ", lines)
-	fmt.Println("Index: ", pathIndex, " | Size: ", len(lines))
-
 	robotPoint := Point{x, y}
 	prevRobotPoint := Point{prevX, prevY}
 
-	for _, line := range lines {
-		fmt.Println("Did ", line, " get past here?")
-		fmt.Println("Previous Loc: ", prevRobotPoint)
+	for _, line := range lineMap {
+		if Debug {
+			fmt.Println("Did ", line, " get past here?")
+			fmt.Println("Previous Loc: ", prevRobotPoint)
+		}
 		if !line.pointOnLine(prevRobotPoint) {
-			fmt.Println("\t", line, " contains ", robotPoint, " => ", line.pointOnLine(robotPoint))
+			if Debug {
+				fmt.Println("\t", line, " contains ", robotPoint, " => ", line.pointOnLine(robotPoint))
+			}
 			if line.pointOnLine(robotPoint) {
 				return line, true
 			}
@@ -477,11 +523,11 @@ func (this *Map) AddWallByLine(degree, distance float64) {
 		fmt.Println("Adding Wall @", x, y)
 	}
 	this.AddWall(x, y, true)
-	this.MarkLineAsSeen(degree, distance)
+	this.MarkLineAsSeen(degree, distance, x, y)
 }
 
 // MarkLineAsSeen marks anything the line passes through as "seen".
-func (this *Map) MarkLineAsSeen(degree, distance float64) {
+func (this *Map) MarkLineAsSeen(degree, distance float64, wallX, wallY int) {
 	if Debug {
 		fmt.Println(RobotMap)
 		RobotMap.Print(nil)
@@ -499,31 +545,25 @@ func (this *Map) MarkLineAsSeen(degree, distance float64) {
 			if this.seen[y][x] == 0 {
 				this.seen[y][x] = 1
 			}
+			if y != wallY && x != wallX {
+				this.floor[y][x] = false
+			}
 		}
 	}
 }
 
 // Counts the ammount of seen tiles (that are just empty space) surrounding the current tile (horizontaly and vertically)
 func (this *Map) getAdjacentSeenTilesCount(x, y int) (count int) {
 	count = 0
-	if y+1 < len(this.floor) {
-		if this.seen[y+1][x] == 1 {
-			count++
-		}
-	}
-	if x+1 < len(this.floor[y]) {
-		if this.seen[y][x+1] == 1 {
-			count++
-		}
-	}
-	if y-1 >= 0 {
-		if this.seen[y-1][x] == 1 {
-			count++
-		}
-	}
-	if x-1 >= 0 {
-		if this.seen[y][x-1] == 1 {
-			count++
+	for yOffset := -1; yOffset <= 1; yOffset++ {
+		for xOffset := -1; xOffset <= 1; xOffset++ {
+			if !(yOffset == 0 && xOffset == 0) {
+				if y+yOffset >= 0 && x+xOffset >= 0 && y+yOffset < len(this.floor) && x+xOffset < len(this.floor[0]) {
+					if this.seen[y+yOffset][x+xOffset] == 1 {
+						count++
+					}
+				}
+			}
 		}
 	}
 	return
@@ -570,7 +610,6 @@ func (this *Map) ContinueToNextArea() {
 			this.MoveRobotAlongPath(path, true)
 			return
 		}
-		print("No valid path to node", i, "checking next node.")
 	}
 	if !possible {
 		finishedMapping = true