v = 0 k = 0 g = 0 kd = 0 err = 0 errold = 0 flag = 1 Sub lineG Motor.ResetCount("BC") While Motor.GetCount("C")10 err = Sensor.ReadPercent(1)-Sensor.ReadPercent(2) int = err up = err*k+(err-errold)*kd+int*0.001 Motor.StartPower("B",-(v+up)) Motor.StartPower("C",v-up) errold = err EndWhile Motor.Stop("BC","True") EndSub Sub right Motor.MoveSync("BC",-50,-50,150,"True") Motor.Stop("BC","True") While Sensor.ReadPercent(2)>10 Motor.StartPower("B",-40) Motor.StartPower("C",-40) EndWhile Motor.MoveSync("BC",-40,-40,20,"True") EndSub v = 45 g = 40 k = 1.2 kd = 12 Mailbox.Connect("robik") mb=Mailbox.Create("Main") Sub Main Motor.StartPower("D",90) Program.Delay(1000) lineG() If flag = 1 Then lineP() flag = 0 EndIf v = 85 g = 10500 k = 3.5 kd = 85 Speaker.Note(100,"C4",100) Mailbox.Send("robik","Main","ok1") lineG() Speaker.Note(100,"C4",100) lineP() Speaker.Note(100,"C4",100) g = 500 lineG() Speaker.Note(100,"C4",100) right() Motor.StartPower("D",-30) Program.Delay(1000) Motor.MoveSync("BC",30,-30,200,"True") g = 400 lineG() Mailbox.Send("robik","Main","ok") right() Program.Delay(1000) lineP() EndSub While "True" Main() status=Mailbox.Receive(mb) Speaker.Tone(100,100,100) EndWhile