first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of BEAR_Motion by
Diff: main.cpp
- Revision:
- 2:9f279c68ed0c
- Parent:
- 0:451c27e4d55e
- Child:
- 4:fd06f8fdec19
--- a/main.cpp Mon Dec 07 14:41:42 2015 +0000 +++ b/main.cpp Mon Dec 07 17:09:33 2015 +0000 @@ -1,5 +1,5 @@ //*****************************************************/ -// Include // +// Include // #include "mbed.h" #include "pinconfig.h" #include "PID.h" @@ -7,7 +7,7 @@ #include "eeprom.h" //*****************************************************/ -// Defines // +// Defines // #define Rate 0.01 #define Kc -2.6 #define Ti 0.0 @@ -17,9 +17,23 @@ // Global // //-- pc monitor -- Serial PC(D1,D0); -//-- encoder -- +//-- encoder -- int Position; int data; + + +//******************************************************* + + +//write encoder +EEPROM memory(I2C_SDA,I2C_SCL,0); +int8_t out,write; +int mod; +InterruptIn send(D11); +int FromSerial; + + +//******************************************************* SPI device(Emosi, Emiso, Esck); DigitalOut Encoder(EncoderA); //-- Motor -- @@ -33,7 +47,7 @@ SPI device(Emosi, Emiso, Esck); device.format(8,0); device.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k - + Encoder = 0; wait_us(50); device.write(0x41); @@ -43,63 +57,191 @@ data = device.write(0x00); wait_us(50); Encoder = 1; - + } //*****************************************************/ void Get_EnValue(int Val) { int i = 0; static unsigned char codes[] = { - 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175, - 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215, - 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235, - 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245, - 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250, - 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125, - 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, - 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 }; - - while(i<=127) - { - if(Val == codes[i]) - { + 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175, + 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215, + 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235, + 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245, + 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250, + 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125, + 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, + 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 + }; + + while(i<=127) { + if(Val == codes[i]) { Position = i; break; + } else i++; + } +} +//****************************************************************************************** +void eewrite() +{ + write = Position; + memory.write(0xFF,write); + PC.printf("%x\n",write); + + wait_ms(10); +} +//****************************************************************************************** +void eeread() +{ + memory.read(0xFF,out); + PC.printf("%x\n",out); + + wait_ms(10); +} +//****************************************************************************************** +void StartWriteRead(int mod) +{ + while(1) { + if(mod==1) { + send.rise(eewrite); + wait_ms(10); + } else if(mod==0) { + send.rise(eeread); + wait_ms(10); } - else i++; } } //*****************************************************/ int main() { - LeftUpper.period(0.00005); - LU_PID.setInputLimits(0,127); - LU_PID.setOutputLimits(0,0.9); - LU_PID.setMode(AUTO_MODE); - - //get the target position - SetPoint = 63; - LU_PID.setSetPoint(SetPoint); - - Read_Encoder(); - PC.printf("%d\n",data); - Get_EnValue(data); - PC.printf("%d\n********************\n",Position); - - while(Position != SetPoint) - { - LU_PID.setProcessValue(Position); - LeftUpper.speed(LU_PID.compute()); - - Read_Encoder(); - Get_EnValue(data); - } - - -} - - - - + + uint8_t state_menu=0; + uint8_t state_show=0; + uint8_t state_exit =0; + uint8_t data; + + // myled = 0; + PC.printf("Wellcome !\n"); + while(1) { + if(state_show == 0) { + PC.printf("Menu\n"); + PC.printf("1.Mode Write Or Read \n"); + PC.printf("2.Logic Input Test\n"); + state_show =1; + } + if(PC.readable()) { + data = PC.getc(); + PC.printf("\n"); + state_show =0; + state_exit =0; + + switch(data) { + case '1': + do { + if(state_menu == 0) { + PC.printf("Mode Write Or Read\n"); + PC.printf("a.Mode Write \n"); + PC.printf("d.Mode Read \n"); + PC.printf("s.Stop \n"); + state_menu = 1; + } + if(PC.readable()) { + data = PC.getc(); + PC.printf("\n"); + state_menu=0; + + switch(data) { + + case 'a': + StartWriteRead(1); + break; + + case 's': + state_exit = 1; + + break; + + case 'd': + StartWriteRead(0); + break; + + default: + PC.printf("plz select a or d\n"); + PC.printf("\n\n"); + break; + + } + } + + } while(state_exit ==0); + PC.printf("\n\n"); + break; + + case '2': + + do { + if(state_menu == 0) { + PC.printf("Mode POP\n\n"); + //***start mode + { + LeftUpper.period(0.00005); + LU_PID.setInputLimits(0,127); + LU_PID.setOutputLimits(0,0.9); + LU_PID.setMode(AUTO_MODE); + + //get the target position + SetPoint = 63; + LU_PID.setSetPoint(SetPoint); + + Read_Encoder(); + PC.printf("%d\n",data); + Get_EnValue(data); + PC.printf("%d\n********************\n",Position); + + while(Position != SetPoint) { + LU_PID.setProcessValue(Position); + LeftUpper.speed(LU_PID.compute()); + + Read_Encoder(); + Get_EnValue(data); + } + } + PC.printf("press x to exit or else to continue\n"); + state_menu = 1; + } + if(PC.readable()) { + data = PC.getc(); + if (data == 'x') state_exit = 1; + else state_menu = 0; + } + + + + } while(state_exit ==0); + PC.printf("\n\n"); + break; + + + case 0x00: + + break; + + default: + PC.printf("plz select 1 or 2 only\n"); + PC.printf("\n\n"); + break; + } + } + + } + + +} + + + + + +