Qian Yuyang / Mbed 2 deprecated Balance_Car_QianYuyang

QAQ ==!

Dependencies:   mbed QEI-1 nRF24L01P xiugai

JY901.cpp@15:934289377f7a, 2019-12-14 (annotated)

Committer:
AlexQian
Date:
Sat Dec 14 05:45:16 2019 +0000
Revision:
15:934289377f7a
Parent:
14:5bc7349d0e3b 
Balance_Car;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zhangyx 14:5bc7349d0e3b 1
zhangyx 14:5bc7349d0e3b 2 #include "JY901.h"
zhangyx 14:5bc7349d0e3b 3 void JY901::receiveData()
zhangyx 14:5bc7349d0e3b 4 {
zhangyx 14:5bc7349d0e3b 5 char ch;
zhangyx 14:5bc7349d0e3b 6 while(mod.readable()){
zhangyx 14:5bc7349d0e3b 7 ch = mod.getc();
zhangyx 14:5bc7349d0e3b 8 parseInput(&ch, 1);
zhangyx 14:5bc7349d0e3b 9 }
zhangyx 14:5bc7349d0e3b 10 }
zhangyx 14:5bc7349d0e3b 11 void JY901::parseCmpt(int token, unsigned char* payloadBuf, int payloadLen)
zhangyx 14:5bc7349d0e3b 12 {
zhangyx 14:5bc7349d0e3b 13 switch(token){
zhangyx 14:5bc7349d0e3b 14 case 0x51:
zhangyx 14:5bc7349d0e3b 15 for (int i = 0; i < 3; ++i)
zhangyx 14:5bc7349d0e3b 16 {
zhangyx 14:5bc7349d0e3b 17 acc[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
zhangyx 14:5bc7349d0e3b 18 acc[i] = acc[i] * 16 * 9.8 / 32768;
zhangyx 14:5bc7349d0e3b 19 }
zhangyx 14:5bc7349d0e3b 20 // lcd.cls();
zhangyx 14:5bc7349d0e3b 21 // lcd.printf("Ax=%d\n",int(data[0]*100));
zhangyx 14:5bc7349d0e3b 22 //pc.printf("Ax=%.2f\tAy=%.2f\tAz=%.2f\r\n", data[0], data[1], data[2]);
zhangyx 14:5bc7349d0e3b 23 break;
zhangyx 14:5bc7349d0e3b 24 case 0x52:
zhangyx 14:5bc7349d0e3b 25 for (int i = 0; i < 3; ++i)
zhangyx 14:5bc7349d0e3b 26 {
zhangyx 14:5bc7349d0e3b 27 gyo[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
zhangyx 14:5bc7349d0e3b 28 gyo[i] = gyo[i] * 2000 / 32768;
zhangyx 14:5bc7349d0e3b 29 }
zhangyx 14:5bc7349d0e3b 30 //pc.printf("Wx=%.2f\tWy=%.2f\tWz=%.2f\r\n", data[0], data[1], data[2]);
zhangyx 14:5bc7349d0e3b 31 break;
zhangyx 14:5bc7349d0e3b 32 case 0x53:
zhangyx 14:5bc7349d0e3b 33 for (int i = 0; i < 3; ++i)
zhangyx 14:5bc7349d0e3b 34 {
zhangyx 14:5bc7349d0e3b 35 att[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
zhangyx 14:5bc7349d0e3b 36 att[i] = att[i] * 180 / 32768;
zhangyx 14:5bc7349d0e3b 37 }
zhangyx 14:5bc7349d0e3b 38 // pc.printf("Roll=%.2f\tPitch=%.2f\tYaw=%.2f\r\n", data[0], data[1], data[2]);
zhangyx 14:5bc7349d0e3b 39 break;
zhangyx 14:5bc7349d0e3b 40 case 0x54:
zhangyx 14:5bc7349d0e3b 41 for (int i = 0; i < 3; ++i)
zhangyx 14:5bc7349d0e3b 42 {
zhangyx 14:5bc7349d0e3b 43 mag[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
zhangyx 14:5bc7349d0e3b 44 }
zhangyx 14:5bc7349d0e3b 45 //pc.printf("Hx=%.2f\tHy=%.2f\tHz=%.2f\r\n", data[0], data[1], data[2]);
zhangyx 14:5bc7349d0e3b 46 break;
zhangyx 14:5bc7349d0e3b 47 }
zhangyx 14:5bc7349d0e3b 48 }
zhangyx 14:5bc7349d0e3b 49 void JY901::parseInput(const char* data, int len)
zhangyx 14:5bc7349d0e3b 50 {
zhangyx 14:5bc7349d0e3b 51 for (int i = 0; i < len; ++i)
zhangyx 14:5bc7349d0e3b 52 {
zhangyx 14:5bc7349d0e3b 53 unsigned char ch = data[i], sum;
zhangyx 14:5bc7349d0e3b 54 switch(state){
zhangyx 14:5bc7349d0e3b 55 case 0:
zhangyx 14:5bc7349d0e3b 56 if(ch == 0x55)
zhangyx 14:5bc7349d0e3b 57 state = 1;
zhangyx 14:5bc7349d0e3b 58 break;
zhangyx 14:5bc7349d0e3b 59 case 1:
zhangyx 14:5bc7349d0e3b 60 token = ch;
zhangyx 14:5bc7349d0e3b 61 if(0x51 <= token && token <= 0x54){
zhangyx 14:5bc7349d0e3b 62 payloadLen = 8;
zhangyx 14:5bc7349d0e3b 63 recvLen = 0;
zhangyx 14:5bc7349d0e3b 64 state = 2;
zhangyx 14:5bc7349d0e3b 65 }else{
zhangyx 14:5bc7349d0e3b 66 // pc.printf("%s %x\r\n", "unknown token", token);
zhangyx 14:5bc7349d0e3b 67 state = 0;
zhangyx 14:5bc7349d0e3b 68 }
zhangyx 14:5bc7349d0e3b 69 break;
zhangyx 14:5bc7349d0e3b 70 case 2:
zhangyx 14:5bc7349d0e3b 71 payloadBuf[recvLen++] = ch;
zhangyx 14:5bc7349d0e3b 72 if(recvLen == payloadLen){
zhangyx 14:5bc7349d0e3b 73 state = 3;
zhangyx 14:5bc7349d0e3b 74 }
zhangyx 14:5bc7349d0e3b 75 break;
zhangyx 14:5bc7349d0e3b 76 case 3:
zhangyx 14:5bc7349d0e3b 77 sum = 0x55;
zhangyx 14:5bc7349d0e3b 78 sum += token;
zhangyx 14:5bc7349d0e3b 79 for (int i = 0; i < payloadLen; ++i)
zhangyx 14:5bc7349d0e3b 80 {
zhangyx 14:5bc7349d0e3b 81 sum += payloadBuf[i];
zhangyx 14:5bc7349d0e3b 82 }
zhangyx 14:5bc7349d0e3b 83 if(sum != ch){
zhangyx 14:5bc7349d0e3b 84 // pc.printf("wrong checksum\r\n");
zhangyx 14:5bc7349d0e3b 85 }else{
zhangyx 14:5bc7349d0e3b 86 parseCmpt(token, payloadBuf, payloadLen);
zhangyx 14:5bc7349d0e3b 87 //myled = !myled;
zhangyx 14:5bc7349d0e3b 88 }
zhangyx 14:5bc7349d0e3b 89 state = 0;
zhangyx 14:5bc7349d0e3b 90 break;
zhangyx 14:5bc7349d0e3b 91 }
zhangyx 14:5bc7349d0e3b 92 }
zhangyx 14:5bc7349d0e3b 93 }