Digital B14
qwq
Dependencies: Motor PID eeprom mbed
Fork of
DogPID
by 
Digital B14
Diff: main.cpp
- Revision:
- 1:9f279c68ed0c
- Parent:
- 0:451c27e4d55e
- Child:
- 2:885fe7eca7d6
--- 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;
+ }
+ }
+
+ }
+
+
+}
+
+
+
+
+
+