4관절로봇제어

/*

ShiftLCD Library - Hello World

Demonstrates the use a 16x2 LCD display. The ShiftLCD library works with

all LCD displays that are compatible with the Hitachi HD44780 driver.

There are many of them out there, and you can usually tell them by the

16-pin interface.

This sketch prints "Hello World!" to the LCD

and shows the time.

The circuit:

---Shift Register 74HC595---

* SR Pin 14 to Arduino pin 2

* SR Pin 12 to Arduino pin 3

* SR Pin 11 to Arduino pin 4

* SR Pin 8 to Ground

* SR Pin 16 to +5v

* SR Pin 13 to Ground

* SR Pin 10 to +5v

-----Shift Reg to LCD--------

* SR Pin 15 to D7

* SR Pin 1 to D6

* SR Pin 2 to D5

* SR Pin 3 to D4

* SR Pin 5 to MOSFET gate

* SR Pin 6 to Enable

* SR Pin 7 to RS

-----LCD HD44780-------------

* Vss to Ground

* Vdd to +5V

* Vo to 10k Wiper

* R/W to Ground

* 5v to +5v

* Gnd to MOSFET Drain

------N Chanel MOSFET--------

* Source to Ground

* Gate to SP Pin 5

* Drain to LCD Gnd

* 1k Resistor Between gate and source

For a more detailed schematic, please see my blog:

http://cjparish.blogspot.com/2010/01/controlling-lcd-display-with-shift.html

Library modified from the original LiquidCrystal Library

This example originaly by Tom Igoe, Jul 2009

Example modified for use with ShiftLCD

Chris Parish, January 12th 2010

*/

// include the library code:

#include <ShiftLCD.h>

#include <Servo.h>

// initialize the library with the numbers of the interface pins

ShiftLCD lcd(2, 4, 3);

Servo myservo1; // create servo object to control a servo

Servo myservo2; // create servo object to control a servo

Servo myservo3; // create servo object to control a servo

Servo myservo4; // create servo object to control a servo

int b1 = 0;

int b2 = 0;

int bt1=0;

int bt2=0;

int bm1=0;

int potpin[] ={0, 1, 2, 3};

int pv[4];

int pvm[]={0,0,0,0};

int val;

int mode=0;

int m=0;

int MM1[10];

int MM2[10];

int MM3[10];

int MM4[10];

int i=0;

int is=0;

int x;

void setup() {

// set up the LCD's number of rows and columns:

lcd.begin(16, 2);

myservo1.attach(8);

myservo2.attach(9);

myservo3.attach(10);

myservo4.attach(11);

pinMode(6, INPUT);

pinMode(7, INPUT);

clearpvm();

}

void loop() {

//------------------------------------------

b1 = digitalRead(6);

b2 = digitalRead(7);

for (i=0; i<4; i++){

pv[i] = analogRead(potpin[i]);

pv[i]= map(pv[i], 0, 1023, 0, 180);

}

pressb1();

pressb2();

if (bm1 <=1){

m=0;

}

//--------------------------------------------

if ( bm1 == 0){

m=0;

lcd.setCursor(0,0);

lcd.print("Menu: ");

lcd.setCursor(0,1);

lcd.print("1.RUN 2.MEM ");

}

//----------------------------------------------

if ( bm1 == 1){

delay(250);

if (digitalRead(6)== LOW){

setservo();

}

else{

bm1=0;

}

is=is+1;

if (is>9){

is=0;

}

delay(250);

}

//-------------------------------------------------

if ( bm1 == 2 ){

x=0;

for (i=0; i<4; i++){

if (pv[i]!= pvm[i]){

x=1;

}

}

if (x == 1){

lcd.setCursor(0,0);

lcd.print("Memory:");

lcd.setCursor(8,0);

lcd.print(m);

lcd.setCursor(0,1);

lcd.print(" ");

for (i=0; i<4; i++){

int z=3-i;

int xa=i*4;

lcd.setCursor(xa,1);

lcd.print(pv[z]);

pvm[z]=pv[z];

}

myservo1.write(pv[0]);

myservo2.write(pv[1]);

myservo3.write(pv[2]);

myservo4.write(pv[3]);

delay(15);

}

}

//---------------------------------------------

if ( bm1 == 3 ){

MM1[m]=pv[0];

MM2[m]=pv[1];

MM3[m]=pv[2];

MM4[m]=pv[3];

lcd.clear();

lcd.setCursor(0,0);

lcd.print("Memory>");

lcd.setCursor(8,0);

lcd.print(m);

lcd.setCursor(0,1);

lcd.print("save OK");

if (m <9){

m= m + 1;

}

else{

m=0;

}

clearpvm();

bm1=2;

delay(200);

}

}

//-------------------------------------------------------

void pressb1(){

if (millis() - bt1 >120 && b1 == HIGH){

lcd.clear();

bt1 = millis();

if (bm1 < 1){

bm1 = 1;

is=0;

}

else{

bm1 = 0;

}

}

}

void pressb2(){

if (millis() - bt2 >120 && b2 == HIGH){

lcd.clear();

clearpvm();

bt2 = millis();

if (bm1 != 1){

if (bm1 <2){

bm1 = 2;

}

else{

bm1 = 3;

}

}

}

}

void setservo(){

lcd.setCursor(0,0);

lcd.print("Run 1.Stop M> ");

lcd.setCursor(14,0);

lcd.print(is);

myservo1.write(MM1[is]);

myservo2.write(MM2[is]);

myservo3.write(MM3[is]);

myservo4.write(MM4[is]);

lcd.setCursor(0,1);

lcd.print(" ");

lcd.setCursor(0,1);

lcd.print(MM4[is]);

lcd.setCursor(4,1);

lcd.print(MM3[is]);

lcd.setCursor(8,1);

lcd.print(MM2[is]);

lcd.setCursor(12,1);

lcd.print(MM1[is]);

}

void clearpvm(){

for (i=0; i<4; i++){

pvm[i]=0;

}

}