Monday, March 31, 2014
Saturday, March 22, 2014
Serial Servo Controller VB GUI Part-3
This Servo Sequencer connects to a serial port where AVR microcontroller is listening for servo inputs .
GUI has 6 scrollbar and a sequencer table where commands can be created/saved/loaded back and can be run to create a set of motion it has speech recognition used microsoft's speech synthesis bundle
GUI screenshot :
code looks something like this
' Nataraja Gadikota
' Serial servo controller program
'VB.NET2008
'controller for 6 servo motor's
'255 steps
Imports System
Imports System.ComponentModel
Imports System.Threading
Imports System.Windows.Forms
Imports System.Speech
Public Class Form1
Dim WithEvents reco As New Recognition.SpeechRecognizer
Dim sv(7) As Byte
Dim sd(7) As Byte
Dim str As String
Dim a As Integer
Dim delay As Integer
Dim lv As Integer
Dim numlistbox As Integer
Dim runflag As Integer
Dim dec As Integer
Dim newposflag As Integer
Dim tx As Integer
Dim synth As New Synthesis.SpeechSynthesizer
Dim w As IO.StreamWriter
Dim r As IO.StreamReader
Private Delegate Sub AddListBoxItemInvoker(ByVal item As Object)
Private Sub Form1_Load(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles MyBase.Load
Dim grammer1 As New Recognition.SrgsGrammar.SrgsDocument
Dim commandRule As New Recognition.SrgsGrammar.SrgsRule("color")
Dim commandList As New Recognition.SrgsGrammar.SrgsOneOf("left", "right", "stop", "up", "down", "home", "center")
synth.SpeakAsync("list of commands " + "left " + "right " + "stop " + "up " + "down " + "home " + "center ")
commandRule.Add(commandList)
grammer1.Rules.Add(commandRule)
grammer1.Root = commandRule
reco.LoadGrammar(New Recognition.Grammar(grammer1))
SerialPort1.PortName = "com1"
SerialPort1.BaudRate = 38400
SerialPort1.Parity = IO.Ports.Parity.None
SerialPort1.StopBits = IO.Ports.StopBits.One
SerialPort1.DataBits = 8
Button2.Text = "close com1 port"
Button1.Text = "open com1 port"
Button3.Text = "send data"
Button13.Text = "save data"
Button14.Text = "load data"
Button12.Text = "add seq"
Button15.Text = "clear"
TextBox8.Text = 0
TextBox9.Text = 0
VScrollBar7.Maximum = 50
VScrollBar1.Maximum = 264
VScrollBar1.Minimum = 1
VScrollBar2.Maximum = 264
VScrollBar2.Minimum = 1
VScrollBar3.Maximum = 264
VScrollBar3.Minimum = 1
VScrollBar4.Maximum = 264
VScrollBar4.Minimum = 1
VScrollBar5.Maximum = 264
VScrollBar5.Minimum = 1
VScrollBar6.Maximum = 264
VScrollBar6.Minimum = 1
VScrollBar7.Minimum = 8
VScrollBar7.Value = 8
VScrollBar6.Value = 127
VScrollBar5.Value = 127
VScrollBar4.Value = 127
VScrollBar3.Value = 127
VScrollBar2.Value = 127
VScrollBar1.Value = 127
Button4.Text = "center"
Button5.Text = "center"
Button6.Text = "center"
Button7.Text = "center"
Button8.Text = "center"
Button9.Text = "center"
Button10.Text = "All home"
Button11.Text = "RUN"
numlistbox = 0
a = 0
lv = 1
dec = 0
runflag = 0
Timer1.Interval = 5
Timer1.Enabled = True
End Sub
Private Sub Button2_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button2.Click
If SerialPort1.IsOpen = True Then SerialPort1.Close()
If SerialPort1.IsOpen = False Then MsgBox("com1 port closed sucessfully")
End Sub
Private Sub Button1_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button1.Click
If SerialPort1.IsOpen = False Then SerialPort1.Open()
If SerialPort1.IsOpen = True Then MsgBox("com1 port opened sucessfully")
End Sub
Private Sub SerialPort1_DataReceived(ByVal sender As Object, _
ByVal e As IO.Ports.SerialDataReceivedEventArgs) Handles SerialPort1.DataReceived
Dim Avalue As Integer
Avalue = SerialPort1.ReadByte
SerialPort1.Write(sv, 0, 7)
Me.AddListBoxItem(Avalue)
End Sub
Private Sub AddListBoxItem(ByVal item As Object)
If Me.TextBox1.InvokeRequired Then
Me.TextBox1.Invoke(New AddListBoxItemInvoker(AddressOf AddListBoxItem), item)
Else
'Me.TextBox10.Text = "received"
Me.TextBox1.Text = (item)
Me.newposflag = 1
End If
End Sub
Private Sub Button3_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button3.Click
SerialPort1.Write(sv, 0, 7)
End Sub
Private Sub VScrollBar1_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles VScrollBar1.ValueChanged
TextBox2.Text = VScrollBar1.Value
sv(0) = Val(VScrollBar1.Value)
End Sub
Private Sub VScrollBar2_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles VScrollBar2.ValueChanged
TextBox3.Text = VScrollBar2.Value
sv(1) = VScrollBar2.Value
End Sub
Private Sub VScrollBar3_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles VScrollBar3.ValueChanged
TextBox4.Text = VScrollBar3.Value
sv(2) = VScrollBar3.Value
End Sub
Private Sub VScrollBar4_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles VScrollBar4.ValueChanged
TextBox5.Text = VScrollBar4.Value
sv(3) = VScrollBar4.Value
End Sub
Private Sub VScrollBar5_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles VScrollBar5.ValueChanged
TextBox6.Text = VScrollBar5.Value
sv(4) = VScrollBar5.Value
End Sub
Private Sub VScrollBar6_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles VScrollBar6.ValueChanged
TextBox7.Text = VScrollBar6.Value
sv(5) = VScrollBar6.Value
End Sub
Private Sub VScrollBar7_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles VScrollBar7.ValueChanged
TextBox9.Text = VScrollBar7.Value
sv(6) = VScrollBar7.Value
End Sub
Private Sub Button4_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button4.Click
VScrollBar1.Value = 127
End Sub
Private Sub Button5_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button5.Click
VScrollBar2.Value = 127
End Sub
Private Sub Button6_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button6.Click
VScrollBar3.Value = 127
End Sub
Private Sub Button7_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button7.Click
VScrollBar4.Value = 127
End Sub
Private Sub Button8_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button8.Click
VScrollBar5.Value = 127
End Sub
Private Sub Button9_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button9.Click
VScrollBar6.Value = 127
End Sub
Private Sub Button10_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button10.Click
VScrollBar6.Value = 127
VScrollBar5.Value = 127
VScrollBar4.Value = 127
VScrollBar3.Value = 127
VScrollBar2.Value = 127
VScrollBar1.Value = 127
End Sub
Private Sub Button11_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button11.Click
'Dim synth1 As New Synthesis.SpeechSynthesizer
'synth1.SpeakAsync("left right center up down")
'MessageBox.Show("run clicked")
runflag = 1
a = 0
dec = 0
'MessageBox.Show(ListBox1.Items.Item(1))
End Sub
Private Sub SetColor(ByVal color As System.Drawing.Color)
Dim synth As New Synthesis.SpeechSynthesizer
synth.SpeakAsync("command executed" + color.ToString)
'Me.BackColor = color
End Sub
Private Sub reco_SpeechRecognized(ByVal sender As Object, ByVal e As System.Speech.Recognition.RecognitionEventArgs) Handles reco.SpeechRecognized
Select Case e.Result.Text
Case "left"
If VScrollBar1.Value > 21 Then
VScrollBar1.Value = VScrollBar1.Value - 20
Else
VScrollBar1.Value = 1
End If
synth.SpeakAsync("command executed" + " left ")
'SetColor(Color.left)
'SerialPort1.Write("red")
Case "right"
If VScrollBar1.Value < 230 Then
VScrollBar1.Value = VScrollBar1.Value + 20
Else
VScrollBar1.Value = 250
End If
synth.SpeakAsync("command executed" + " right ")
'SetColor(Color.right)
'SerialPort1.Write("green")
Case "fast"
If VScrollBar7.Value >= 6 Then
VScrollBar7.Value = VScrollBar7.Value - 1
Else
VScrollBar7.Value = 5
End If
synth.SpeakAsync("command executed" + " fast ")
'SetColor(Color.fast)
'SerialPort1.Write("blue")
Case "slow"
If VScrollBar7.Value <= 15 Then
VScrollBar7.Value = VScrollBar7.Value + 1
Else
VScrollBar7.Value = 16
End If
synth.SpeakAsync("command executed" + " slow ")
'SetColor(Color.slow)
'SerialPort1.Write("pink")
Case "center"
VScrollBar1.Value = 127
synth.SpeakAsync("command executed" + " center ")
'SetColor(Color.center)
'SerialPort1.Write("yellow")
Case "stop"
VScrollBar1.Value = VScrollBar1.Value
synth.SpeakAsync("command executed" + " stop ")
'SetColor(Color.stop)
Case "up"
VScrollBar1.Value = 1
synth.SpeakAsync("command executed" + " up ")
Case "down"
VScrollBar1.Value = 254
synth.SpeakAsync("command executed" + " down ")
End Select
End Sub
Private Sub TextBox9_TextChanged(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles TextBox9.TextChanged
End Sub
Private Sub Button12_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button12.Click
Dim str1 As String
str1 = TextBox2.Text + " " + TextBox3.Text + " " + TextBox4.Text + " " + TextBox5.Text + " " + TextBox6.Text + " " + TextBox7.Text + " " + TextBox9.Text
ListBox1.Items.Add(str1)
numlistbox = numlistbox + 1
'MessageBox.Show(numlistbox)
End Sub
Private Sub Timer1_Tick(ByVal sender As Object, ByVal e As System.EventArgs) Handles Timer1.Tick
Dim cnt As Integer
Me.VScrollBar6.Value = sv(5)
Me.VScrollBar5.Value = sv(4)
Me.VScrollBar4.Value = sv(3)
Me.VScrollBar3.Value = sv(2)
Me.VScrollBar2.Value = sv(1)
Me.VScrollBar1.Value = sv(0)
tx = Val(TextBox8.Text)
TextBox10.Text = tx
If newposflag = 1 Then
If ((runflag = 1) And (numlistbox > a)) Then
If lv = 1 Then
Me.str = ListBox1.Items.Item(a)
cnt = 0
For Each j As Short In Me.str.Split(" ")
sd(cnt) = Val(j)
cnt = cnt + 1
Next
delay = sd(6)
TextBox11.Text = delay
lv = 0
Else
If dec = 0 Then
For cnt = 0 To 5 Step 1
If ((tx <> 0) And (sv(cnt) <> sd(cnt))) Then
If (sd(cnt) > sv(cnt) And (sv(cnt) + tx < sd(cnt))) Then
sv(cnt) = sv(cnt) + tx
ElseIf (sd(cnt) < sv(cnt) And (sv(cnt) > sd(cnt) + tx)) Then
sv(cnt) = sv(cnt) - tx
Else
sv(cnt) = sd(cnt)
End If
Else
sv(cnt) = sd(cnt)
End If
Next cnt
For cnt = 0 To 5 Step 1
If (sv(cnt) = sd(cnt)) Then
If cnt = 5 Then
dec = 1
End If
Else
Exit For
End If
Next cnt
End If
If dec = 1 Then
If delay = 0 Then
a = a + 1
lv = 1
dec = 0
Else
delay = delay - 1
End If
End If
End If
Else
runflag = 0
End If
newposflag = 0
End If
End Sub
Private Sub Button13_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button13.Click
'save to file
Dim userMsg As String
userMsg = Microsoft.VisualBasic.InputBox("enter the file name with path ", "save file As", "C:\VBtest\")
Dim li As Integer
w = New IO.StreamWriter(userMsg)
For li = 0 To ListBox1.Items.Count - 1
w.WriteLine(ListBox1.Items.Item(li))
Next
w.Close()
End Sub
Private Sub Button14_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button14.Click
'load to listbox
Dim testMsg As Integer
Dim userMsg As String
testMsg = MsgBox("to u want to append data ", vbYesNoCancel + vbExclamation, "append or clear")
If testMsg <> 6 Then
ListBox1.Items.Clear()
End If
userMsg = Microsoft.VisualBasic.InputBox("enter the file name with path ", "load from ", "C:\VBtest\")
r = New IO.StreamReader(userMsg)
While (r.Peek() > -1)
ListBox1.Items.Add(r.ReadLine)
End While
r.Close()
End Sub
Private Sub Button15_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button15.Click
'clear list box
ListBox1.Items.Clear()
End Sub
End Class
Serial Servo Controller Part-2
Hello ,
I've blah blah'd a lot in my previous blog regarding servo controller more than 2 year ago .
http://miniprojects-be-btech.blogspot.in/2011/08/serial-servo-controller.html
Now I would like to share the code , incidentally its my Btech final year project .
schedular based serial servo controller based on AVR & GUI has been developed using VB ( will share the GUI details in next blog ) , GUI can read command from a file something like a sequencer .
Code goes like this .
#include
#include
#include
#define F_CPU 16000000UL
#include
#include
#define SETBIT(ADDRESS, BIT) (ADDRESS |= (1 << BIT))
#define CLEARBIT(ADDRESS, BIT) (ADDRESS &= ~(1 << BIT))
/// Data Types /////////
typedef unsigned char u8;
typedef unsigned int u16;
u8 prev;
u8 position[8],count=0;
typedef struct task
{
// pointer to a function
void (*pfunc) (void);
// delay before the first call
u16 delay;
// interval between subsequent runs
u16 period;
// flag indicating time to run
u8 run;
}task;
/// Defines ///////////
// 25msec period 256-180
// 7.3728MHz and /1024 prescaler
#define StartFrom 200
// maximum number of tasks
#define MAXnTASKS 3
/// Globals ///////////
volatile task TaskArray[MAXnTASKS];
void InitScheduler (void);
void DeleteTask (u8 index);
void AddTask (void (*taskfunc)(void), u16 taskdelay, u16 taskperiod);
void DispatchTask (void);
void PWM (void);
void get1 (void);
void USARTWriteChar(unsigned char data)
{
//Wait untill the transmitter is ready
while(!(UCSRA & (1< {
//Do nothing
}
//Now write the data to USART buffer
UDR=data;
}
char USARTReadChar(void)
{
while ( !(UCSRA & (1<{
}
return UDR;
}
void USARTInit(uint16_t ubrr_value)
{
//Set Baud rate
UBRRL = ubrr_value;
UBRRH = (ubrr_value>>8);
DDRD=1< DDRA=255;
/*Set Frame Format
>> Asynchronous mode
>> No Parity
>> 1 StopBit
>> char size 8
*/
UCSRC=(1<
//Enable The receiver and transmitter
UCSRB=(1<
}
int main()
{
InitScheduler();
DDRA=0xff;
DDRB=0;
PORTB=255;
USARTInit(103);
_delay_ms(10);
// populate task array
position[7]=1;
position[6]=1;
position[5]=1;
position[4]=1;
position[3]=1;
position[2]=1;
position[1]=1;
position[0]=1;
AddTask (PWM, 0, 8);
AddTask (get1, 1, 8);
// enable interrupts
// sei();
cli();
while (1)
{
get1 ();
}
}
void InitScheduler (void)
{
u8 i;
// timer prescaler clock/1024
TCCR0 |= (1< // clear pending interrupts
TIFR = 1< // enable timer0 overflow interrupt
TIMSK |= 1< // load timer0
TCNT0 = StartFrom;
// clear task array
for (i=0; i}
void DeleteTask (u8 j)
{
TaskArray[j].pfunc = 0x0000;
TaskArray[j].delay = 0;
TaskArray[j].period = 0;
TaskArray[j].run = 0;
}
void AddTask (void (*taskfunc)(void), u16 taskdelay, u16 taskperiod)
{
u8 n=0;
// find next available position
while ((TaskArray[n].pfunc != 0) && (n < MAXnTASKS)) n++;
// place task
if (n < MAXnTASKS)
{
TaskArray[n].pfunc = taskfunc;
TaskArray[n].delay = taskdelay;
TaskArray[n].period = taskperiod;
TaskArray[n].run = 0;
}
}
SIGNAL(SIG_OVERFLOW0)
{
u8 m;
// testing
// load timer
TCNT0 = StartFrom;
for (m=0; m {
if (TaskArray[m].pfunc)
{
if (TaskArray[m].delay == 0)
{
TaskArray[m].run = 1;
TaskArray[m].delay = TaskArray[m].period;
}
else TaskArray[m].delay--;
}
}
}
void DispatchTask (void)
{
u8 k;
for (k=0; k {
if (TaskArray[k].run == 1)
{
// run task
(*TaskArray[k].pfunc)();
// clear run flag
TaskArray[k].run = 0;
}
}
}
void PWM (void)
{
cli();
int tnow;
if(1)
{
SETBIT(PORTA, 0);
}
if(1)
{
SETBIT(PORTA, 1);
}
if(1)
{
SETBIT(PORTA, 2);
}
if(1)
{
SETBIT(PORTA, 3);
}
if(1)
{
SETBIT(PORTA, 4);
}
if(1)
{
SETBIT(PORTA, 5);
}
if(1)
{
SETBIT(PORTA, 6);
}
if(1)
{
SETBIT(PORTA, 7);
}
_delay_us(597);
for(tnow=0;tnow<128 p="" tnow="">{
if(position[7]==tnow)
{
CLEARBIT(PORTA, 0);
}
if(position[6]==tnow)
{
CLEARBIT(PORTA, 1);
}
if(position[5]==tnow)
{
CLEARBIT(PORTA, 2);
}
if(position[4]==tnow)
{
CLEARBIT(PORTA, 3);
}
if(position[3]==tnow)
{
CLEARBIT(PORTA, 4);
}
if(position[2]==tnow)
{
CLEARBIT(PORTA, 5);
}
if(position[1]==tnow)
{
CLEARBIT(PORTA, 6);
}
if(position[0]==tnow)
{
CLEARBIT(PORTA, 7);
}
_delay_us(5);
}// end of tnow
sei();
}
void get1 (void)
{
int i;
unsigned int send=position[0];
send=send+10;
USARTWriteChar(send);
//USARTWriteChar('\');
//USARTWriteChar('n');
for(i=0;i<1 i="" p="">position[i]=USARTReadChar();
}
I've blah blah'd a lot in my previous blog regarding servo controller more than 2 year ago .
http://miniprojects-be-btech.blogspot.in/2011/08/serial-servo-controller.html
Now I would like to share the code , incidentally its my Btech final year project .
schedular based serial servo controller based on AVR & GUI has been developed using VB ( will share the GUI details in next blog ) , GUI can read command from a file something like a sequencer .
Code goes like this .
#include
#include
#include
#define F_CPU 16000000UL
#include
#include
#define SETBIT(ADDRESS, BIT) (ADDRESS |= (1 << BIT))
#define CLEARBIT(ADDRESS, BIT) (ADDRESS &= ~(1 << BIT))
/// Data Types /////////
typedef unsigned char u8;
typedef unsigned int u16;
u8 prev;
u8 position[8],count=0;
typedef struct task
{
// pointer to a function
void (*pfunc) (void);
// delay before the first call
u16 delay;
// interval between subsequent runs
u16 period;
// flag indicating time to run
u8 run;
}task;
/// Defines ///////////
// 25msec period 256-180
// 7.3728MHz and /1024 prescaler
#define StartFrom 200
// maximum number of tasks
#define MAXnTASKS 3
/// Globals ///////////
volatile task TaskArray[MAXnTASKS];
void InitScheduler (void);
void DeleteTask (u8 index);
void AddTask (void (*taskfunc)(void), u16 taskdelay, u16 taskperiod);
void DispatchTask (void);
void PWM (void);
void get1 (void);
void USARTWriteChar(unsigned char data)
{
//Wait untill the transmitter is ready
while(!(UCSRA & (1<
//Do nothing
}
//Now write the data to USART buffer
UDR=data;
}
char USARTReadChar(void)
{
while ( !(UCSRA & (1<
}
return UDR;
}
void USARTInit(uint16_t ubrr_value)
{
//Set Baud rate
UBRRL = ubrr_value;
UBRRH = (ubrr_value>>8);
DDRD=1<
/*Set Frame Format
>> Asynchronous mode
>> No Parity
>> 1 StopBit
>> char size 8
*/
UCSRC=(1<
//Enable The receiver and transmitter
UCSRB=(1<
}
int main()
{
InitScheduler();
DDRA=0xff;
DDRB=0;
PORTB=255;
USARTInit(103);
_delay_ms(10);
// populate task array
position[7]=1;
position[6]=1;
position[5]=1;
position[4]=1;
position[3]=1;
position[2]=1;
position[1]=1;
position[0]=1;
AddTask (PWM, 0, 8);
AddTask (get1, 1, 8);
// enable interrupts
// sei();
cli();
while (1)
{
get1 ();
}
}
void InitScheduler (void)
{
u8 i;
// timer prescaler clock/1024
TCCR0 |= (1<
TIFR = 1<
TIMSK |= 1<
TCNT0 = StartFrom;
// clear task array
for (i=0; i
void DeleteTask (u8 j)
{
TaskArray[j].pfunc = 0x0000;
TaskArray[j].delay = 0;
TaskArray[j].period = 0;
TaskArray[j].run = 0;
}
void AddTask (void (*taskfunc)(void), u16 taskdelay, u16 taskperiod)
{
u8 n=0;
// find next available position
while ((TaskArray[n].pfunc != 0) && (n < MAXnTASKS)) n++;
// place task
if (n < MAXnTASKS)
{
TaskArray[n].pfunc = taskfunc;
TaskArray[n].delay = taskdelay;
TaskArray[n].period = taskperiod;
TaskArray[n].run = 0;
}
}
SIGNAL(SIG_OVERFLOW0)
{
u8 m;
// testing
// load timer
TCNT0 = StartFrom;
for (m=0; m
if (TaskArray[m].pfunc)
{
if (TaskArray[m].delay == 0)
{
TaskArray[m].run = 1;
TaskArray[m].delay = TaskArray[m].period;
}
else TaskArray[m].delay--;
}
}
}
void DispatchTask (void)
{
u8 k;
for (k=0; k
if (TaskArray[k].run == 1)
{
// run task
(*TaskArray[k].pfunc)();
// clear run flag
TaskArray[k].run = 0;
}
}
}
void PWM (void)
{
cli();
int tnow;
if(1)
{
SETBIT(PORTA, 0);
}
if(1)
{
SETBIT(PORTA, 1);
}
if(1)
{
SETBIT(PORTA, 2);
}
if(1)
{
SETBIT(PORTA, 3);
}
if(1)
{
SETBIT(PORTA, 4);
}
if(1)
{
SETBIT(PORTA, 5);
}
if(1)
{
SETBIT(PORTA, 6);
}
if(1)
{
SETBIT(PORTA, 7);
}
_delay_us(597);
for(tnow=0;tnow<128 p="" tnow="">{
if(position[7]==tnow)
{
CLEARBIT(PORTA, 0);
}
if(position[6]==tnow)
{
CLEARBIT(PORTA, 1);
}
if(position[5]==tnow)
{
CLEARBIT(PORTA, 2);
}
if(position[4]==tnow)
{
CLEARBIT(PORTA, 3);
}
if(position[3]==tnow)
{
CLEARBIT(PORTA, 4);
}
if(position[2]==tnow)
{
CLEARBIT(PORTA, 5);
}
if(position[1]==tnow)
{
CLEARBIT(PORTA, 6);
}
if(position[0]==tnow)
{
CLEARBIT(PORTA, 7);
}
_delay_us(5);
}// end of tnow
sei();
}
void get1 (void)
{
int i;
unsigned int send=position[0];
send=send+10;
USARTWriteChar(send);
//USARTWriteChar('\');
//USARTWriteChar('n');
for(i=0;i<1 i="" p="">position[i]=USARTReadChar();
}
Neural Networks Matlab
neural networks matlab
using NNTOOL
here we will be training a network to recognize a particular pattern
first we have to normalize the data.......
let the data be (X)
2 5 6 3 1
5 2 2 5 1
4 4 5 4 4
1 5 2 4 5
1 6 6 6 6
3 6 3 2 1
6 4 2 5 4
3 1 2 5 3
4 1 4 3 1
2 2 3 4 3
each column a particular pattern
first find the minimum value from each column and subtract
the same then divide the whole column with the max value (column)
ull get data something like this (x)
0.5822 0.5447 0.4046 0.6963 0.3477
0.5407 0.6473 0.4484 0.0938 0.1500
0.8699 0.5439 0.3658 0.5254 0.5861
0.2648 0.7210 0.7635 0.5303 0.2621
0.3181 0.5225 0.6279 0.8611 0.0445
0.1192 0.9937 0.7720 0.4849 0.7549
0.9398 0.2187 0.9329 0.3935 0.2428
0.6456 0.1058 0.9727 0.6714 0.4424
0.4795 0.1097 0.1920 0.7413 0.6878
0.6393 0.0636 0.1389 0.5201 0.3592
let the output of each pattern be(y)
0.7363 0.3947 0.6834 0.7040 0.4423
output corresponding to each column,normalized
x(:,1)-->y(1,1)
x(:,2)-->y(1,2) lly
type nntool at matlab command window another window named
Network/Data Manager will open (1.jpg)
click on import button and export x as input data and y as target data !
half way !!! ;)
ull get a message like this (2.jpg)
after this close import window ,now ur network/data manager window will look like this (3.JPG)
now click on new button ull see another window here u have to select the type of NN u want to create
we will be create a feedforward back propagation NN most popular one
der u select the inputs and target data (4.JPG)
then click on create and close that window
now u should be able to see a network in network/data manager window(middle)
click on that newly created network (icon) then click on open
ull see another window (5.jpg)
click on train tab -> then training info
add inputs and targets as x,y then click on train network ....wait for some time ull see a graph once the
goal is met click on stop training button and close that graph and this window ..
now our NN is trained and is read for testing ........ hurray
after this click on export button in network/data manager ....
now close this window ..u should be able to see ur trained network in ur matlab work space......
now use this command
output=sim("networkname","data")
example
output=sim(network1,x(:,1))
which should be same as y(1,1)
u can test ur network reliability by adding noise .....
ur rand function for this purpose
thats it ........
thks
using NNTOOL
here we will be training a network to recognize a particular pattern
first we have to normalize the data.......
let the data be (X)
2 5 6 3 1
5 2 2 5 1
4 4 5 4 4
1 5 2 4 5
1 6 6 6 6
3 6 3 2 1
6 4 2 5 4
3 1 2 5 3
4 1 4 3 1
2 2 3 4 3
each column a particular pattern
first find the minimum value from each column and subtract
the same then divide the whole column with the max value (column)
ull get data something like this (x)
0.5822 0.5447 0.4046 0.6963 0.3477
0.5407 0.6473 0.4484 0.0938 0.1500
0.8699 0.5439 0.3658 0.5254 0.5861
0.2648 0.7210 0.7635 0.5303 0.2621
0.3181 0.5225 0.6279 0.8611 0.0445
0.1192 0.9937 0.7720 0.4849 0.7549
0.9398 0.2187 0.9329 0.3935 0.2428
0.6456 0.1058 0.9727 0.6714 0.4424
0.4795 0.1097 0.1920 0.7413 0.6878
0.6393 0.0636 0.1389 0.5201 0.3592
let the output of each pattern be(y)
0.7363 0.3947 0.6834 0.7040 0.4423
output corresponding to each column,normalized
x(:,1)-->y(1,1)
x(:,2)-->y(1,2) lly
type nntool at matlab command window another window named
Network/Data Manager will open (1.jpg)
click on import button and export x as input data and y as target data !
half way !!! ;)
ull get a message like this (2.jpg)
after this close import window ,now ur network/data manager window will look like this (3.JPG)
now click on new button ull see another window here u have to select the type of NN u want to create
we will be create a feedforward back propagation NN most popular one
der u select the inputs and target data (4.JPG)
then click on create and close that window
now u should be able to see a network in network/data manager window(middle)
click on that newly created network (icon) then click on open
ull see another window (5.jpg)
click on train tab -> then training info
add inputs and targets as x,y then click on train network ....wait for some time ull see a graph once the
goal is met click on stop training button and close that graph and this window ..
now our NN is trained and is read for testing ........ hurray
after this click on export button in network/data manager ....
now close this window ..u should be able to see ur trained network in ur matlab work space......
now use this command
output=sim("networkname","data")
example
output=sim(network1,x(:,1))
which should be same as y(1,1)
u can test ur network reliability by adding noise .....
ur rand function for this purpose
thats it ........
thks
Tuesday, January 7, 2014
single plane maze router with obstacle
TCL code for a single place maze router which considers obstacle while routing to destination .
set heap_sort ""
proc add_val {val} {
global heap_sort;
global MAX_X;
global MAX_Y;
## PUSH only valid values
if { [lindex $val 0 1 0] < 0 || [lindex $val 0 1 0] > $MAX_X ||
[lindex $val 0 1 1] < 0 || [lindex $val 0 1 1] > $MAX_Y } {
return ;
}
set heap_sort "$heap_sort $val "
set heap_sort [lsort -index 0 -integer $heap_sort]
puts "$heap_sort "
}
proc get_val {} {
global heap_sort;
set val ""
if { [llength $heap_sort] < 1 } {
puts $val
return $val
}
set val [lindex $heap_sort 0]
if { [llength $heap_sort] > 1 } {
puts "deleting 0th element"
set heap_sort [lrange $heap_sort 1 end]
} else {
puts "deleting 0th element"
set heap_sort ""
}
puts $val
return $val
}
proc find_paths {file} {
global MAX_X;
global MAX_Y;
set total_paths 0
set f [open $file r]
#Just File Processing to simplify the input to be given
while { ! [eof $f] } {
gets $f str
if { [regexp {^SIZE\s*(\d+)\,(\d+)} $str mv MAX_X MAX_Y] } {
} elseif {[regexp {^START\s*(\d+)\,(\d+)} $str mv C_X C_Y ] } {
} elseif {[regexp {^S\s*(\d+)\,(\d+)} $str mv tx ty] } {
# This is a 2D array in TCL, remembering the solidier co-ordinates
set soldier($tx,$ty) 1
}
}
# File Processing Done
set num 0
#Assuming Castle moves Down first
set dir DOWN
set cnt 0
#C_X and C_Y are the initial castle co-ordinates
#add_val is a proc like a function in C
#add_val arguments are {weight {Castlelocationx castlelocationy CastleDirection} {ourPath}}
add_val "{0 {$C_X $C_Y DOWN} {}}"
# while to make sure we dont go to infinite loop
while { $num < 10000 } {
#get_val is a proc like a function in c, this function returns the first element of the list and changes the list to remove the first element
set c_node [get_val]
if { $c_node == "" } {
puts "Heap Empty , processing DONE "
break ;
}
#puts "Expanding $c_node"
set c_wave_path [lindex $c_node 2]
set dir [lindex $c_node 1 2 ]
set c_x [lindex $c_node 1 0 ]
set c_y [lindex $c_node 1 1 ]
set n_c_y $c_y
set n_c_x $c_x
if {$dir == "DOWN" } {
set n_c_y [expr $c_y + 1]
set n_dir "RIGHT"
} elseif { $dir == "UP" } {
set n_c_y [expr $c_y - 1]
set n_dir "LEFT"
} elseif { $dir == "LEFT" } {
set n_c_x [expr $c_x - 1]
set n_dir "DOWN"
} elseif { $dir == "RIGHT" } {
set n_c_x [expr $c_x + 1]
set n_dir "UP"
}
if { [info exists soldier($n_c_x,$n_c_y)] } {
add_val "{[expr [lindex $c_node 0 0] + 30 ] {$n_c_x $n_c_y $n_dir} {$c_wave_path {$c_x $c_y}}}"
}
if { $n_c_x == $C_X && $n_c_y == $C_Y } {
puts "Reached Castle"
set c_wave_path_t [lindex $c_node 2]
set c_wave_path_t "$c_wave_path_t {$n_c_x $n_c_y}"
puts "Path is \n[join $c_wave_path_t \"\n\"]"
incr total_paths
continue
}
add_val "{[expr [lindex $c_node 0 0] + 1 ] {$n_c_x $n_c_y $dir} {$c_wave_path {$c_x $c_y}}}"
incr num
}
puts "Total Number of Paths to Castle $total_paths"
}
#calling Function "find_paths" for which input is file called "abc"
find_paths abc
set heap_sort ""
proc add_val {val} {
global heap_sort;
global MAX_X;
global MAX_Y;
## PUSH only valid values
if { [lindex $val 0 1 0] < 0 || [lindex $val 0 1 0] > $MAX_X ||
[lindex $val 0 1 1] < 0 || [lindex $val 0 1 1] > $MAX_Y } {
return ;
}
set heap_sort "$heap_sort $val "
set heap_sort [lsort -index 0 -integer $heap_sort]
puts "$heap_sort "
}
proc get_val {} {
global heap_sort;
set val ""
if { [llength $heap_sort] < 1 } {
puts $val
return $val
}
set val [lindex $heap_sort 0]
if { [llength $heap_sort] > 1 } {
puts "deleting 0th element"
set heap_sort [lrange $heap_sort 1 end]
} else {
puts "deleting 0th element"
set heap_sort ""
}
puts $val
return $val
}
proc find_paths {file} {
global MAX_X;
global MAX_Y;
set total_paths 0
set f [open $file r]
#Just File Processing to simplify the input to be given
while { ! [eof $f] } {
gets $f str
if { [regexp {^SIZE\s*(\d+)\,(\d+)} $str mv MAX_X MAX_Y] } {
} elseif {[regexp {^START\s*(\d+)\,(\d+)} $str mv C_X C_Y ] } {
} elseif {[regexp {^S\s*(\d+)\,(\d+)} $str mv tx ty] } {
# This is a 2D array in TCL, remembering the solidier co-ordinates
set soldier($tx,$ty) 1
}
}
# File Processing Done
set num 0
#Assuming Castle moves Down first
set dir DOWN
set cnt 0
#C_X and C_Y are the initial castle co-ordinates
#add_val is a proc like a function in C
#add_val arguments are {weight {Castlelocationx castlelocationy CastleDirection} {ourPath}}
add_val "{0 {$C_X $C_Y DOWN} {}}"
# while to make sure we dont go to infinite loop
while { $num < 10000 } {
#get_val is a proc like a function in c, this function returns the first element of the list and changes the list to remove the first element
set c_node [get_val]
if { $c_node == "" } {
puts "Heap Empty , processing DONE "
break ;
}
#puts "Expanding $c_node"
set c_wave_path [lindex $c_node 2]
set dir [lindex $c_node 1 2 ]
set c_x [lindex $c_node 1 0 ]
set c_y [lindex $c_node 1 1 ]
set n_c_y $c_y
set n_c_x $c_x
if {$dir == "DOWN" } {
set n_c_y [expr $c_y + 1]
set n_dir "RIGHT"
} elseif { $dir == "UP" } {
set n_c_y [expr $c_y - 1]
set n_dir "LEFT"
} elseif { $dir == "LEFT" } {
set n_c_x [expr $c_x - 1]
set n_dir "DOWN"
} elseif { $dir == "RIGHT" } {
set n_c_x [expr $c_x + 1]
set n_dir "UP"
}
if { [info exists soldier($n_c_x,$n_c_y)] } {
add_val "{[expr [lindex $c_node 0 0] + 30 ] {$n_c_x $n_c_y $n_dir} {$c_wave_path {$c_x $c_y}}}"
}
if { $n_c_x == $C_X && $n_c_y == $C_Y } {
puts "Reached Castle"
set c_wave_path_t [lindex $c_node 2]
set c_wave_path_t "$c_wave_path_t {$n_c_x $n_c_y}"
puts "Path is \n[join $c_wave_path_t \"\n\"]"
incr total_paths
continue
}
add_val "{[expr [lindex $c_node 0 0] + 1 ] {$n_c_x $n_c_y $dir} {$c_wave_path {$c_x $c_y}}}"
incr num
}
puts "Total Number of Paths to Castle $total_paths"
}
#calling Function "find_paths" for which input is file called "abc"
find_paths abc
Fuzzy logic Autonomous Car
Hosting TCL code for the vedio , http://www.youtube.com/watch?v=4r9Ou-JC4ao
package require Tk
proc get_slope {value} {
global INPUT_DOMAIN
global OUTPUT_DOMAIN
if {$value <= [lindex $INPUT_DOMAIN 0] } {
# puts "enering this loop1 [lindex $INPUT_DOMAIN 0] <= $value"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 0]]
} elseif {$value > [lindex $INPUT_DOMAIN 0] && $value < [lindex $INPUT_DOMAIN 1] } {
# puts "enering this loop2 [expr abs([expr [lindex $INPUT_DOMAIN 1] - $value])]"
set int_with_0 [expr [expr abs([expr [lindex $INPUT_DOMAIN 1] - $value])] / 10.0]
set int_with_1 [expr [expr abs([expr [lindex $INPUT_DOMAIN 0] - $value])] / 10.0]
set angle_0 [expr $int_with_0 * [lindex $OUTPUT_DOMAIN 0]]
set angle_1 [expr $int_with_1 * [lindex $OUTPUT_DOMAIN 1]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value == [lindex $INPUT_DOMAIN 1]} {
# puts "enering this loop3"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 1]]
} elseif {$value > [lindex $INPUT_DOMAIN 1] && $value < [lindex $INPUT_DOMAIN 2] } {
# puts "enering this loop4"
set int_with_1 [expr [expr abs([expr [lindex $INPUT_DOMAIN 2] - $value])] / 10.0]
set int_with_2 [expr [expr abs([expr [lindex $INPUT_DOMAIN 1] - $value])] / 10.0]
set angle_0 [expr $int_with_1 * [lindex $OUTPUT_DOMAIN 1]]
set angle_1 [expr $int_with_2 * [lindex $OUTPUT_DOMAIN 2]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value == [lindex $INPUT_DOMAIN 2]} {
# puts "enering this loop5"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 2]]
} elseif {$value > [lindex $INPUT_DOMAIN 2] && $value < [lindex $INPUT_DOMAIN 3] } {
# puts "enering this loop6"
set int_with_2 [expr [expr abs([expr [lindex $INPUT_DOMAIN 3] - $value])] / 10.0]
set int_with_3 [expr [expr abs([expr [lindex $INPUT_DOMAIN 2] - $value])] / 10.0]
set angle_0 [expr $int_with_2 * [lindex $OUTPUT_DOMAIN 2]]
set angle_1 [expr $int_with_3 * [lindex $OUTPUT_DOMAIN 3]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value == [lindex $INPUT_DOMAIN 3]} {
# puts "enering this loop7"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 3]]
} elseif {$value > [lindex $INPUT_DOMAIN 3] && $value < [lindex $INPUT_DOMAIN 4] } {
# puts "enering this loop8"
set int_with_3 [expr [expr abs([expr [lindex $INPUT_DOMAIN 4] - $value])] / 10.0]
set int_with_4 [expr [expr abs([expr [lindex $INPUT_DOMAIN 3] - $value])] / 10.0]
set angle_0 [expr $int_with_3 * [lindex $OUTPUT_DOMAIN 3]]
set angle_1 [expr $int_with_4 * [lindex $OUTPUT_DOMAIN 4]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value >= [lindex $INPUT_DOMAIN 4]} {
# puts "enering this loop9"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 4]]
}
return $angle
#puts "Angle for $value is $angle"
}
proc xvalue { loc angle} {
set x [lindex $loc 0]
set y [lindex $loc 1]
return [expr ($x * cos(($angle*3.14159265)/180)) - ($y * sin(($angle*3.14159265)/180)) ]
}
proc yvalue { loc angle } {
set x [lindex $loc 0]
set y [lindex $loc 1]
return [expr ($y * cos(($angle*3.14159265)/180)) + ($x * sin(($angle*3.14159265)/180)) ]
}
proc update_car { can loc angle } {
global LEFT_SENSOR
global RIGHT_SENSOR
catch { $can delete car }
#catch { $can delete line }
set newx [xvalue $loc -$angle]
set newy [yvalue $loc -$angle]
set temp_loc "[expr $newx - 30] [expr $newy - 15]"
set line_corrds "[xvalue $temp_loc $angle] [yvalue $temp_loc $angle] "
set temp_loc "[expr $newx + 30] [expr $newy - 15]"
set LEFT_SENSOR "[xvalue $temp_loc $angle] [yvalue $temp_loc $angle]"
eval "$can create rect [lindex $LEFT_SENSOR 0] [lindex $LEFT_SENSOR 1] [expr [lindex $LEFT_SENSOR 0] + 10] [expr [lindex $LEFT_SENSOR 1] + 10] -fill blue -tag car"
set line_corrds "$line_corrds $LEFT_SENSOR "
puts "right sensor $LEFT_SENSOR"
set temp_loc "[expr $newx + 30] [expr $newy + 15]"
set RIGHT_SENSOR "[xvalue $temp_loc $angle] [yvalue $temp_loc $angle]"
eval "$can create rect [lindex $RIGHT_SENSOR 0] [lindex $RIGHT_SENSOR 1] [expr [lindex $RIGHT_SENSOR 0] + 10] [expr [lindex $RIGHT_SENSOR 1] + 10] -fill yellow -tag car"
set line_corrds "$line_corrds $RIGHT_SENSOR "
set temp_loc "[expr $newx - 30] [expr $newy + 15]"
set line_corrds "$line_corrds [xvalue $temp_loc $angle] [yvalue $temp_loc $angle] "
eval "$can create polygon $line_corrds -fill green -tag car"
#eval "$can create line
}
#set save_file [open road.txt r]
#close $save_file
proc addLine {x y} {
global side_lines ;
set save_file [open road.txt a]
puts $save_file ".canvas create line [.canvas canvasx $::lastx] [.canvas canvasy $::lasty] [.canvas canvasx $x] [.canvas canvasy $y]"
puts $save_file "set side_lines \"\$side_lines { {[.canvas canvasx $::lastx] [.canvas canvasy $::lasty]} {[.canvas canvasx $x] [.canvas canvasy $y]} } \" "
close $save_file
.canvas create line [.canvas canvasx $::lastx] [.canvas canvasy $::lasty] [.canvas canvasx $x] [.canvas canvasy $y]
set side_lines "$side_lines { {[.canvas canvasx $::lastx] [.canvas canvasy $::lasty]} {[.canvas canvasx $x] [.canvas canvasy $y]} } "
set ::lastx $x; set ::lasty $y
}
#############################################################################
proc Intersect {p1 p2 p3 p4} {
return [IntersectV $p1 [VSub $p2 $p1] $p3 [VSub $p4 $p3]]
}
proc IntersectV {p1 v1 p3 v3} {
foreach {x1 y1} $p1 {vx1 vy1} $v1 {x3 y3} $p3 {vx3 vy3} $v3 break
set a $vx1
set b [expr {-1 * $vx3}]
set c $vy1
set d [expr {-1 * $vy3}]
set e [expr {$x3 - $x1}]
set f [expr {$y3 - $y1}]
set det [expr {double($a*$d - $b*$c)}]
if {$det == 0} {return 0}
set k [expr {($d*$e - $b*$f) / $det}]
#set j [expr {($a*$f - $c*$e) / $det}]
return [VAdd $p1 $v1 $k]
}
proc VAdd {v1 v2 {scaling 1}} {
foreach {x1 y1} $v1 {x2 y2} $v2 break
return [list [expr {$x1 + $scaling*$x2}] [expr {$y1 + $scaling*$y2}]]
}
proc VSub {v1 v2} { return [VAdd $v1 $v2 -1] }
proc VCross {v1 v2} {
foreach {x1 y1} $v1 {x2 y2} $v2 break
return [expr {($x1*$y2) - ($y1*$x2)}]
}
proc VRotate {v beta} {
foreach {x y} $v break
set xx [expr {$x * cos(-$beta) - $y * sin(-$beta)}]
set yy [expr {$x * sin(-$beta) + $y * cos(-$beta)}]
return [list $xx $yy]
}
proc sensor_reading {POS DIR DEPTH tag} {
global side_lines ;
set POS_EXT "[expr [lindex $POS 0] + ($DEPTH *cos($DIR * (3.14159265 /180) ))] [expr [lindex $POS 1] + ($DEPTH *sin($DIR * (3.14159265 /180) ))] "
set current_reading 100000
# puts ".canvas create line [lindex $POS 0] [lindex $POS 1] [lindex $POS_EXT 0] [lindex $POS_EXT 1] -tag sensor_lines"
.canvas create line [lindex $POS 0] [lindex $POS 1] [lindex $POS_EXT 0] [lindex $POS_EXT 1] -tag sensor_lines
foreach line_seg $side_lines {
foreach {C1 C2} $line_seg { break }
set result [Intersect $POS $POS_EXT $C1 $C2 ]
if { $result == 0 } { continue }
set xmax -100
set ymax -100
set xmin 100000
set ymin 100000
foreach xin "{$POS} {$POS_EXT} {$C1} {$C2}" {
set x [lindex $xin 0]
set y [lindex $xin 1]
if { $xmax < $x } { set xmax $x }
if { $ymax < $y } { set ymax $y }
if { $xmin > $x } { set xmin $x }
if { $ymin > $y } { set ymin $y }
}
set smallest_right_hand_vale [lindex $POS_EXT 0]
if { [lindex $POS_EXT 0] > [lindex $C2 0] } {
set smallest_right_hand_vale [lindex $C2 0]
}
set largest_left_hand_vale [lindex $POS 0]
if { [lindex $POS 0] < [lindex $C1 0] } {
set largest_left_hand_vale [lindex $C1 0]
}
set smallest_right_hand_valey [lindex $POS 1]
if { [lindex $POS 1] > [lindex $C1 1] } {
set smallest_right_hand_valey [lindex $C1 1]
}
set largest_left_hand_valey [lindex $POS_EXT 1]
if { [lindex $POS_EXT 1] < [lindex $C2 1] } {
set largest_left_hand_valey [lindex $C2 1]
}
if { [lindex $C1 0] > [lindex $C2 0] } {
set t_max [lindex $C1 0]
set t_min [lindex $C2 0]
} else {
set t_max [lindex $C2 0]
set t_min [lindex $C1 0]
}
if { [lindex $C1 1] > [lindex $C2 1] } {
set ty_max [lindex $C1 1]
set ty_min [lindex $C2 1]
} else {
set ty_max [lindex $C2 1]
set ty_min [lindex $C1 1]
}
####################
if { [lindex $POS 0] > [lindex $POS_EXT 0] } {
set t_max1 [lindex $POS 0]
set t_min1 [lindex $POS_EXT 0]
} else {
set t_max1 [lindex $POS_EXT 0]
set t_min1 [lindex $POS 0]
}
if { [lindex $POS 1] > [lindex $POS_EXT 1] } {
set ty_max1 [lindex $POS 1]
set ty_min1 [lindex $POS_EXT 1]
} else {
set ty_max1 [lindex $POS_EXT 1]
set ty_min1 [lindex $POS 1]
}
#if { [lindex $result 0] <= $smallest_right_hand_vale && [lindex $result 0] >= $largest_left_hand_vale &&
# [lindex $result 1] <= $largest_left_hand_valey && [lindex $result 1] >= $smallest_right_hand_valey &&
# }
if { $t_min <= [lindex $result 0] && $t_max >= [lindex $result 0] &&
$ty_min <= [lindex $result 1] && $ty_max >= [lindex $result 1] &&
$t_min1 <= [lindex $result 0] && $t_max1 >= [lindex $result 0] &&
$ty_min1 <= [lindex $result 1] && $ty_max1 >= [lindex $result 1]
} {
set temp_x [expr sqrt((([lindex $POS 0] - [lindex $result 0])*([lindex $POS 0] - [lindex $result 0]))+(([lindex $POS 1] - [lindex $result 1])*([lindex $POS 1] - [lindex $result 1])))]
if { $temp_x < $current_reading } {
catch { .canvas delete $tag ;}
eval "catch {.canvas delete sensor_lines_my_$tag}"
eval ".canvas create oval [expr [lindex $result 0] - 2] [expr [lindex $result 1] - 2] [expr [lindex $result 0] + 2] [expr [lindex $result 1] + 2] -fill yellow -tag $tag"
eval ".canvas create line [lindex $C1 0] [lindex $C1 1] [lindex $C2 0] [lindex $C2 1] -fill red -width 10 -tag sensor_lines_my_$tag"
# puts "Changing value to $temp_x from $current_reading because $POS && $result -> $POS $POS_EXT $C1 $C2 "
set current_reading $temp_x
} else {
# puts "Current reading is less than the present valyue $temp_x $current_reading"
}
} else {
# puts "Intersection is invalid $result --xmax $xmax- xmin -$xmin- ymax -$ymax- ymin -$ymin --- "
}
}
puts "Sensor Reading $current_reading"
return $current_reading
}
## FUZZY Logic
proc gaussian_val { x U sig } {
return [expr exp(-(($x-$U)*($x-$U))/(2*($sig*$sig)))]
}
catch { destroy .canvas }
grid [canvas .canvas] -sticky nwes -column 0 -row 0
grid columnconfigure . 0 -weight 1
grid rowconfigure . 0 -weight 1
bind .canvas <1> "set lastx %x; set lasty %y"
bind .canvas "addLine %x %y"
set side_lines ""
set current_loc "100 100"
set current_angle "45"
set INPUT_DOMAIN "-400 -200 0 200 400"
set SIG_DOMAIN_MAP "2.726 2.726 2.726 2.726 2.726"
set OUTPUT_DOMAIN "20 10 0 -10 -20"
set DEPTH 2000
set go_straight 0
proc start_car {} {
global go_straight
global side_lines ;
global RIGHT_SENSOR
global LEFT_SENSOR
global current_loc
global current_angle
global INPUT_DOMAIN
global SIG_DOMAIN_MAP
global OUTPUT_DOMAIN
global DEPTH
set test_cnt 0
while { 1 } {
incr test_cnt
if { $test_cnt > 100 } { break ;}
update_car .canvas $current_loc $current_angle
catch {.canvas delete sensor_lines}
catch {.cavnas delete cir_right}
set sensor1 [sensor_reading $RIGHT_SENSOR $current_angle $DEPTH "cir_right"] ; #Left
catch {.cavnas delete cir_left}
set sensor2 [sensor_reading $LEFT_SENSOR $current_angle $DEPTH "cir_left"] ; #Right
puts "Right Sensor reading $sensor1 LEFT reading $sensor2"
if { $sensor1 == 100000 || $sensor2 == 100000 } { puts "ERROR : got Zero reading for sensor : ending COde " ;
incr go_straight 1
set Theta 0
if { $go_straight > 100 } {
break ;
}
} else {
set go_straight 0
}
if { $go_straight == 0 } {
set diff [expr $sensor2 - $sensor1 ]
set least $sensor1
if { $sensor2 < $least } {
set least $sensor2
}
puts "Got Diff $diff $least "
set diff [expr ($diff / exp(($least-80)/5)) ]
if { [expr abs($diff)] < 0.07 && $least < 20} {
set diff [expr $diff * 600 ]
} elseif { [expr abs($diff)] < 0.1 && $least < 50} {
set diff [expr $diff * 100 ]
} elseif { [expr abs($diff)] < 1 } {
# set diff [expr $diff * 10 ]
} elseif { [expr abs($diff)] < 5 } {
# set diff [expr $diff * 5 ]
}
puts "Got Diff after mult $diff"
# check for upper & lower bounds
if { $diff < [lindex $INPUT_DOMAIN 0] } {
set Theta [lindex $OUTPUT_DOMAIN 0]
} elseif {$diff > [lindex $INPUT_DOMAIN end]} {
set Theta [lindex $OUTPUT_DOMAIN end]
} else {
set cnt -1
set Theta [get_slope $diff]
#foreach x $INPUT_DOMAIN {
# incr cnt
#
# #set Theta [expr $Theta + ([lindex $OUTPUT_DOMAIN $cnt ] * get_slope [gaussian_val $diff $x [lindex $SIG_DOMAIN_MAP $cnt] ] ) ]
# set Theta [expr $Theta + [get_slope $diff] ]
# puts "Current $Theta in $cnt"
#}
}
set current_angle [expr $current_angle + $Theta ]
}
set car_movement_limit 10
puts "Hi $current_loc $Theta $current_angle "
set current_loc "[expr [lindex $current_loc 0] + ($car_movement_limit *cos($current_angle * (3.14159265 /180) ))] [expr [lindex $current_loc 1] + ($car_movement_limit *sin($current_angle * (3.14159265 /180) ))] "
puts "$current_angle $current_loc "
after 300 start_car
break
} ; # end of frame
} ; ## end of start car
if { [file exists road.txt ] } {
source road.txt
start_car
}
package require Tk
proc get_slope {value} {
global INPUT_DOMAIN
global OUTPUT_DOMAIN
if {$value <= [lindex $INPUT_DOMAIN 0] } {
# puts "enering this loop1 [lindex $INPUT_DOMAIN 0] <= $value"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 0]]
} elseif {$value > [lindex $INPUT_DOMAIN 0] && $value < [lindex $INPUT_DOMAIN 1] } {
# puts "enering this loop2 [expr abs([expr [lindex $INPUT_DOMAIN 1] - $value])]"
set int_with_0 [expr [expr abs([expr [lindex $INPUT_DOMAIN 1] - $value])] / 10.0]
set int_with_1 [expr [expr abs([expr [lindex $INPUT_DOMAIN 0] - $value])] / 10.0]
set angle_0 [expr $int_with_0 * [lindex $OUTPUT_DOMAIN 0]]
set angle_1 [expr $int_with_1 * [lindex $OUTPUT_DOMAIN 1]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value == [lindex $INPUT_DOMAIN 1]} {
# puts "enering this loop3"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 1]]
} elseif {$value > [lindex $INPUT_DOMAIN 1] && $value < [lindex $INPUT_DOMAIN 2] } {
# puts "enering this loop4"
set int_with_1 [expr [expr abs([expr [lindex $INPUT_DOMAIN 2] - $value])] / 10.0]
set int_with_2 [expr [expr abs([expr [lindex $INPUT_DOMAIN 1] - $value])] / 10.0]
set angle_0 [expr $int_with_1 * [lindex $OUTPUT_DOMAIN 1]]
set angle_1 [expr $int_with_2 * [lindex $OUTPUT_DOMAIN 2]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value == [lindex $INPUT_DOMAIN 2]} {
# puts "enering this loop5"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 2]]
} elseif {$value > [lindex $INPUT_DOMAIN 2] && $value < [lindex $INPUT_DOMAIN 3] } {
# puts "enering this loop6"
set int_with_2 [expr [expr abs([expr [lindex $INPUT_DOMAIN 3] - $value])] / 10.0]
set int_with_3 [expr [expr abs([expr [lindex $INPUT_DOMAIN 2] - $value])] / 10.0]
set angle_0 [expr $int_with_2 * [lindex $OUTPUT_DOMAIN 2]]
set angle_1 [expr $int_with_3 * [lindex $OUTPUT_DOMAIN 3]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value == [lindex $INPUT_DOMAIN 3]} {
# puts "enering this loop7"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 3]]
} elseif {$value > [lindex $INPUT_DOMAIN 3] && $value < [lindex $INPUT_DOMAIN 4] } {
# puts "enering this loop8"
set int_with_3 [expr [expr abs([expr [lindex $INPUT_DOMAIN 4] - $value])] / 10.0]
set int_with_4 [expr [expr abs([expr [lindex $INPUT_DOMAIN 3] - $value])] / 10.0]
set angle_0 [expr $int_with_3 * [lindex $OUTPUT_DOMAIN 3]]
set angle_1 [expr $int_with_4 * [lindex $OUTPUT_DOMAIN 4]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value >= [lindex $INPUT_DOMAIN 4]} {
# puts "enering this loop9"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 4]]
}
return $angle
#puts "Angle for $value is $angle"
}
proc xvalue { loc angle} {
set x [lindex $loc 0]
set y [lindex $loc 1]
return [expr ($x * cos(($angle*3.14159265)/180)) - ($y * sin(($angle*3.14159265)/180)) ]
}
proc yvalue { loc angle } {
set x [lindex $loc 0]
set y [lindex $loc 1]
return [expr ($y * cos(($angle*3.14159265)/180)) + ($x * sin(($angle*3.14159265)/180)) ]
}
proc update_car { can loc angle } {
global LEFT_SENSOR
global RIGHT_SENSOR
catch { $can delete car }
#catch { $can delete line }
set newx [xvalue $loc -$angle]
set newy [yvalue $loc -$angle]
set temp_loc "[expr $newx - 30] [expr $newy - 15]"
set line_corrds "[xvalue $temp_loc $angle] [yvalue $temp_loc $angle] "
set temp_loc "[expr $newx + 30] [expr $newy - 15]"
set LEFT_SENSOR "[xvalue $temp_loc $angle] [yvalue $temp_loc $angle]"
eval "$can create rect [lindex $LEFT_SENSOR 0] [lindex $LEFT_SENSOR 1] [expr [lindex $LEFT_SENSOR 0] + 10] [expr [lindex $LEFT_SENSOR 1] + 10] -fill blue -tag car"
set line_corrds "$line_corrds $LEFT_SENSOR "
puts "right sensor $LEFT_SENSOR"
set temp_loc "[expr $newx + 30] [expr $newy + 15]"
set RIGHT_SENSOR "[xvalue $temp_loc $angle] [yvalue $temp_loc $angle]"
eval "$can create rect [lindex $RIGHT_SENSOR 0] [lindex $RIGHT_SENSOR 1] [expr [lindex $RIGHT_SENSOR 0] + 10] [expr [lindex $RIGHT_SENSOR 1] + 10] -fill yellow -tag car"
set line_corrds "$line_corrds $RIGHT_SENSOR "
set temp_loc "[expr $newx - 30] [expr $newy + 15]"
set line_corrds "$line_corrds [xvalue $temp_loc $angle] [yvalue $temp_loc $angle] "
eval "$can create polygon $line_corrds -fill green -tag car"
#eval "$can create line
}
#set save_file [open road.txt r]
#close $save_file
proc addLine {x y} {
global side_lines ;
set save_file [open road.txt a]
puts $save_file ".canvas create line [.canvas canvasx $::lastx] [.canvas canvasy $::lasty] [.canvas canvasx $x] [.canvas canvasy $y]"
puts $save_file "set side_lines \"\$side_lines { {[.canvas canvasx $::lastx] [.canvas canvasy $::lasty]} {[.canvas canvasx $x] [.canvas canvasy $y]} } \" "
close $save_file
.canvas create line [.canvas canvasx $::lastx] [.canvas canvasy $::lasty] [.canvas canvasx $x] [.canvas canvasy $y]
set side_lines "$side_lines { {[.canvas canvasx $::lastx] [.canvas canvasy $::lasty]} {[.canvas canvasx $x] [.canvas canvasy $y]} } "
set ::lastx $x; set ::lasty $y
}
#############################################################################
proc Intersect {p1 p2 p3 p4} {
return [IntersectV $p1 [VSub $p2 $p1] $p3 [VSub $p4 $p3]]
}
proc IntersectV {p1 v1 p3 v3} {
foreach {x1 y1} $p1 {vx1 vy1} $v1 {x3 y3} $p3 {vx3 vy3} $v3 break
set a $vx1
set b [expr {-1 * $vx3}]
set c $vy1
set d [expr {-1 * $vy3}]
set e [expr {$x3 - $x1}]
set f [expr {$y3 - $y1}]
set det [expr {double($a*$d - $b*$c)}]
if {$det == 0} {return 0}
set k [expr {($d*$e - $b*$f) / $det}]
#set j [expr {($a*$f - $c*$e) / $det}]
return [VAdd $p1 $v1 $k]
}
proc VAdd {v1 v2 {scaling 1}} {
foreach {x1 y1} $v1 {x2 y2} $v2 break
return [list [expr {$x1 + $scaling*$x2}] [expr {$y1 + $scaling*$y2}]]
}
proc VSub {v1 v2} { return [VAdd $v1 $v2 -1] }
proc VCross {v1 v2} {
foreach {x1 y1} $v1 {x2 y2} $v2 break
return [expr {($x1*$y2) - ($y1*$x2)}]
}
proc VRotate {v beta} {
foreach {x y} $v break
set xx [expr {$x * cos(-$beta) - $y * sin(-$beta)}]
set yy [expr {$x * sin(-$beta) + $y * cos(-$beta)}]
return [list $xx $yy]
}
proc sensor_reading {POS DIR DEPTH tag} {
global side_lines ;
set POS_EXT "[expr [lindex $POS 0] + ($DEPTH *cos($DIR * (3.14159265 /180) ))] [expr [lindex $POS 1] + ($DEPTH *sin($DIR * (3.14159265 /180) ))] "
set current_reading 100000
# puts ".canvas create line [lindex $POS 0] [lindex $POS 1] [lindex $POS_EXT 0] [lindex $POS_EXT 1] -tag sensor_lines"
.canvas create line [lindex $POS 0] [lindex $POS 1] [lindex $POS_EXT 0] [lindex $POS_EXT 1] -tag sensor_lines
foreach line_seg $side_lines {
foreach {C1 C2} $line_seg { break }
set result [Intersect $POS $POS_EXT $C1 $C2 ]
if { $result == 0 } { continue }
set xmax -100
set ymax -100
set xmin 100000
set ymin 100000
foreach xin "{$POS} {$POS_EXT} {$C1} {$C2}" {
set x [lindex $xin 0]
set y [lindex $xin 1]
if { $xmax < $x } { set xmax $x }
if { $ymax < $y } { set ymax $y }
if { $xmin > $x } { set xmin $x }
if { $ymin > $y } { set ymin $y }
}
set smallest_right_hand_vale [lindex $POS_EXT 0]
if { [lindex $POS_EXT 0] > [lindex $C2 0] } {
set smallest_right_hand_vale [lindex $C2 0]
}
set largest_left_hand_vale [lindex $POS 0]
if { [lindex $POS 0] < [lindex $C1 0] } {
set largest_left_hand_vale [lindex $C1 0]
}
set smallest_right_hand_valey [lindex $POS 1]
if { [lindex $POS 1] > [lindex $C1 1] } {
set smallest_right_hand_valey [lindex $C1 1]
}
set largest_left_hand_valey [lindex $POS_EXT 1]
if { [lindex $POS_EXT 1] < [lindex $C2 1] } {
set largest_left_hand_valey [lindex $C2 1]
}
if { [lindex $C1 0] > [lindex $C2 0] } {
set t_max [lindex $C1 0]
set t_min [lindex $C2 0]
} else {
set t_max [lindex $C2 0]
set t_min [lindex $C1 0]
}
if { [lindex $C1 1] > [lindex $C2 1] } {
set ty_max [lindex $C1 1]
set ty_min [lindex $C2 1]
} else {
set ty_max [lindex $C2 1]
set ty_min [lindex $C1 1]
}
####################
if { [lindex $POS 0] > [lindex $POS_EXT 0] } {
set t_max1 [lindex $POS 0]
set t_min1 [lindex $POS_EXT 0]
} else {
set t_max1 [lindex $POS_EXT 0]
set t_min1 [lindex $POS 0]
}
if { [lindex $POS 1] > [lindex $POS_EXT 1] } {
set ty_max1 [lindex $POS 1]
set ty_min1 [lindex $POS_EXT 1]
} else {
set ty_max1 [lindex $POS_EXT 1]
set ty_min1 [lindex $POS 1]
}
#if { [lindex $result 0] <= $smallest_right_hand_vale && [lindex $result 0] >= $largest_left_hand_vale &&
# [lindex $result 1] <= $largest_left_hand_valey && [lindex $result 1] >= $smallest_right_hand_valey &&
# }
if { $t_min <= [lindex $result 0] && $t_max >= [lindex $result 0] &&
$ty_min <= [lindex $result 1] && $ty_max >= [lindex $result 1] &&
$t_min1 <= [lindex $result 0] && $t_max1 >= [lindex $result 0] &&
$ty_min1 <= [lindex $result 1] && $ty_max1 >= [lindex $result 1]
} {
set temp_x [expr sqrt((([lindex $POS 0] - [lindex $result 0])*([lindex $POS 0] - [lindex $result 0]))+(([lindex $POS 1] - [lindex $result 1])*([lindex $POS 1] - [lindex $result 1])))]
if { $temp_x < $current_reading } {
catch { .canvas delete $tag ;}
eval "catch {.canvas delete sensor_lines_my_$tag}"
eval ".canvas create oval [expr [lindex $result 0] - 2] [expr [lindex $result 1] - 2] [expr [lindex $result 0] + 2] [expr [lindex $result 1] + 2] -fill yellow -tag $tag"
eval ".canvas create line [lindex $C1 0] [lindex $C1 1] [lindex $C2 0] [lindex $C2 1] -fill red -width 10 -tag sensor_lines_my_$tag"
# puts "Changing value to $temp_x from $current_reading because $POS && $result -> $POS $POS_EXT $C1 $C2 "
set current_reading $temp_x
} else {
# puts "Current reading is less than the present valyue $temp_x $current_reading"
}
} else {
# puts "Intersection is invalid $result --xmax $xmax- xmin -$xmin- ymax -$ymax- ymin -$ymin --- "
}
}
puts "Sensor Reading $current_reading"
return $current_reading
}
## FUZZY Logic
proc gaussian_val { x U sig } {
return [expr exp(-(($x-$U)*($x-$U))/(2*($sig*$sig)))]
}
catch { destroy .canvas }
grid [canvas .canvas] -sticky nwes -column 0 -row 0
grid columnconfigure . 0 -weight 1
grid rowconfigure . 0 -weight 1
bind .canvas <1> "set lastx %x; set lasty %y"
bind .canvas
set side_lines ""
set current_loc "100 100"
set current_angle "45"
set INPUT_DOMAIN "-400 -200 0 200 400"
set SIG_DOMAIN_MAP "2.726 2.726 2.726 2.726 2.726"
set OUTPUT_DOMAIN "20 10 0 -10 -20"
set DEPTH 2000
set go_straight 0
proc start_car {} {
global go_straight
global side_lines ;
global RIGHT_SENSOR
global LEFT_SENSOR
global current_loc
global current_angle
global INPUT_DOMAIN
global SIG_DOMAIN_MAP
global OUTPUT_DOMAIN
global DEPTH
set test_cnt 0
while { 1 } {
incr test_cnt
if { $test_cnt > 100 } { break ;}
update_car .canvas $current_loc $current_angle
catch {.canvas delete sensor_lines}
catch {.cavnas delete cir_right}
set sensor1 [sensor_reading $RIGHT_SENSOR $current_angle $DEPTH "cir_right"] ; #Left
catch {.cavnas delete cir_left}
set sensor2 [sensor_reading $LEFT_SENSOR $current_angle $DEPTH "cir_left"] ; #Right
puts "Right Sensor reading $sensor1 LEFT reading $sensor2"
if { $sensor1 == 100000 || $sensor2 == 100000 } { puts "ERROR : got Zero reading for sensor : ending COde " ;
incr go_straight 1
set Theta 0
if { $go_straight > 100 } {
break ;
}
} else {
set go_straight 0
}
if { $go_straight == 0 } {
set diff [expr $sensor2 - $sensor1 ]
set least $sensor1
if { $sensor2 < $least } {
set least $sensor2
}
puts "Got Diff $diff $least "
set diff [expr ($diff / exp(($least-80)/5)) ]
if { [expr abs($diff)] < 0.07 && $least < 20} {
set diff [expr $diff * 600 ]
} elseif { [expr abs($diff)] < 0.1 && $least < 50} {
set diff [expr $diff * 100 ]
} elseif { [expr abs($diff)] < 1 } {
# set diff [expr $diff * 10 ]
} elseif { [expr abs($diff)] < 5 } {
# set diff [expr $diff * 5 ]
}
puts "Got Diff after mult $diff"
# check for upper & lower bounds
if { $diff < [lindex $INPUT_DOMAIN 0] } {
set Theta [lindex $OUTPUT_DOMAIN 0]
} elseif {$diff > [lindex $INPUT_DOMAIN end]} {
set Theta [lindex $OUTPUT_DOMAIN end]
} else {
set cnt -1
set Theta [get_slope $diff]
#foreach x $INPUT_DOMAIN {
# incr cnt
#
# #set Theta [expr $Theta + ([lindex $OUTPUT_DOMAIN $cnt ] * get_slope [gaussian_val $diff $x [lindex $SIG_DOMAIN_MAP $cnt] ] ) ]
# set Theta [expr $Theta + [get_slope $diff] ]
# puts "Current $Theta in $cnt"
#}
}
set current_angle [expr $current_angle + $Theta ]
}
set car_movement_limit 10
puts "Hi $current_loc $Theta $current_angle "
set current_loc "[expr [lindex $current_loc 0] + ($car_movement_limit *cos($current_angle * (3.14159265 /180) ))] [expr [lindex $current_loc 1] + ($car_movement_limit *sin($current_angle * (3.14159265 /180) ))] "
puts "$current_angle $current_loc "
after 300 start_car
break
} ; # end of frame
} ; ## end of start car
if { [file exists road.txt ] } {
source road.txt
start_car
}
Subscribe to:
Posts (Atom)