maedalab / Mbed 2 deprecated i-sobot_test

test publish

Dependencies:   mbed

main.cpp@0:caf1cb8a6a1e, 2016-08-01 (annotated)

Committer:
mfurukawa
Date:
Mon Aug 01 08:12:55 2016 +0000
Revision:
0:caf1cb8a6a1e
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mfurukawa 0:caf1cb8a6a1e 1 #include "mbed.h"
mfurukawa 0:caf1cb8a6a1e 2
mfurukawa 0:caf1cb8a6a1e 3 Ticker flipper;
mfurukawa 0:caf1cb8a6a1e 4 Serial pc(USBTX, USBRX);
mfurukawa 0:caf1cb8a6a1e 5
mfurukawa 0:caf1cb8a6a1e 6 DigitalOut dOut_0(P0_17); //right arm
mfurukawa 0:caf1cb8a6a1e 7 //DigitalOut dOut_1(P0_20); //left arm
mfurukawa 0:caf1cb8a6a1e 8 //DigitalOut dOut_2(P0_16); //right leg
mfurukawa 0:caf1cb8a6a1e 9 //DigitalOut dOut_3(P0_2); //left leg
mfurukawa 0:caf1cb8a6a1e 10 DigitalOut dOut_led(P0_0); //left leg
mfurukawa 0:caf1cb8a6a1e 11
mfurukawa 0:caf1cb8a6a1e 12 //motion
mfurukawa 0:caf1cb8a6a1e 13 int data[4][8]; //angle data for servos
mfurukawa 0:caf1cb8a6a1e 14 int tmp[2];
mfurukawa 0:caf1cb8a6a1e 15 //softwareUART
mfurukawa 0:caf1cb8a6a1e 16 unsigned char bitCount,byteCount; //used in softwareUART
mfurukawa 0:caf1cb8a6a1e 17
mfurukawa 0:caf1cb8a6a1e 18 void flip() { //servo drive
mfurukawa 0:caf1cb8a6a1e 19 int sum;
mfurukawa 0:caf1cb8a6a1e 20 unsigned char out[4];
mfurukawa 0:caf1cb8a6a1e 21
mfurukawa 0:caf1cb8a6a1e 22 //calclulate data at start of sending (every 80*0.417msec)
mfurukawa 0:caf1cb8a6a1e 23 if(bitCount == 0 && byteCount == 0){
mfurukawa 0:caf1cb8a6a1e 24 for(int i=0; i<1; i++){
mfurukawa 0:caf1cb8a6a1e 25 data[i][0] = 0xFF; //header
mfurukawa 0:caf1cb8a6a1e 26 data[i][1] = 0x05; //number of data
mfurukawa 0:caf1cb8a6a1e 27 data[i][2] = tmp[0]; //servo1
mfurukawa 0:caf1cb8a6a1e 28 data[i][3] = tmp[0]; //servo2
mfurukawa 0:caf1cb8a6a1e 29 data[i][4] = tmp[0]; //servo3
mfurukawa 0:caf1cb8a6a1e 30 data[i][5] = 128; //servo4
mfurukawa 0:caf1cb8a6a1e 31 data[i][6] = 128; //servo5
mfurukawa 0:caf1cb8a6a1e 32 //calc checksum
mfurukawa 0:caf1cb8a6a1e 33 data[i][7] = data[i][1] + data[i][2] + data[i][3] + data[i][4] + data[i][5] + data[i][6];
mfurukawa 0:caf1cb8a6a1e 34 sum = data[i][7];
mfurukawa 0:caf1cb8a6a1e 35 sum >>= 8;
mfurukawa 0:caf1cb8a6a1e 36 data[i][7] += sum;
mfurukawa 0:caf1cb8a6a1e 37 }
mfurukawa 0:caf1cb8a6a1e 38 }
mfurukawa 0:caf1cb8a6a1e 39
mfurukawa 0:caf1cb8a6a1e 40 //send one bit data (software UART)
mfurukawa 0:caf1cb8a6a1e 41 if(bitCount == 0){
mfurukawa 0:caf1cb8a6a1e 42 out[0] = out[1] = out[2] = out[3] = 0; //start bit
mfurukawa 0:caf1cb8a6a1e 43 }else if(bitCount == 9){
mfurukawa 0:caf1cb8a6a1e 44 out[0] = out[1] = out[2] = out[3] = 1; //stop bit
mfurukawa 0:caf1cb8a6a1e 45 }else{
mfurukawa 0:caf1cb8a6a1e 46 out[0] = (data[0][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
mfurukawa 0:caf1cb8a6a1e 47 out[1] = (data[1][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
mfurukawa 0:caf1cb8a6a1e 48 out[2] = (data[2][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
mfurukawa 0:caf1cb8a6a1e 49 out[3] = (data[3][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
mfurukawa 0:caf1cb8a6a1e 50 }
mfurukawa 0:caf1cb8a6a1e 51 dOut_0 = out[0];
mfurukawa 0:caf1cb8a6a1e 52 //dOut_1 = out[1];
mfurukawa 0:caf1cb8a6a1e 53 // dOut_2 = out[2];
mfurukawa 0:caf1cb8a6a1e 54 // dOut_3 = out[3];
mfurukawa 0:caf1cb8a6a1e 55 //
mfurukawa 0:caf1cb8a6a1e 56 bitCount++;
mfurukawa 0:caf1cb8a6a1e 57 if(bitCount > 9){
mfurukawa 0:caf1cb8a6a1e 58 bitCount = 0;
mfurukawa 0:caf1cb8a6a1e 59 byteCount++;
mfurukawa 0:caf1cb8a6a1e 60 if(byteCount > 7)byteCount = 0;
mfurukawa 0:caf1cb8a6a1e 61 }
mfurukawa 0:caf1cb8a6a1e 62 }
mfurukawa 0:caf1cb8a6a1e 63
mfurukawa 0:caf1cb8a6a1e 64
mfurukawa 0:caf1cb8a6a1e 65 void setup(){
mfurukawa 0:caf1cb8a6a1e 66 pc.baud(115200);
mfurukawa 0:caf1cb8a6a1e 67 bitCount = byteCount = 0;
mfurukawa 0:caf1cb8a6a1e 68 LPC_USART0->CFG |= 1; //Enable USART0
mfurukawa 0:caf1cb8a6a1e 69 }
mfurukawa 0:caf1cb8a6a1e 70
mfurukawa 0:caf1cb8a6a1e 71 void pc_rx() {
mfurukawa 0:caf1cb8a6a1e 72 pc.printf("%d\n",tmp[0]);
mfurukawa 0:caf1cb8a6a1e 73
mfurukawa 0:caf1cb8a6a1e 74 }
mfurukawa 0:caf1cb8a6a1e 75
mfurukawa 0:caf1cb8a6a1e 76 int main() {
mfurukawa 0:caf1cb8a6a1e 77 setup();
mfurukawa 0:caf1cb8a6a1e 78
mfurukawa 0:caf1cb8a6a1e 79 //pc.attach(pc_rx, Serial::RxIrq);
mfurukawa 0:caf1cb8a6a1e 80
mfurukawa 0:caf1cb8a6a1e 81 wait(2.0);
mfurukawa 0:caf1cb8a6a1e 82
mfurukawa 0:caf1cb8a6a1e 83 flipper.attach_us(&flip, 417);
mfurukawa 0:caf1cb8a6a1e 84
mfurukawa 0:caf1cb8a6a1e 85 while(1) {
mfurukawa 0:caf1cb8a6a1e 86 int cnt=0;
mfurukawa 0:caf1cb8a6a1e 87 while( pc.readable())
mfurukawa 0:caf1cb8a6a1e 88 {
mfurukawa 0:caf1cb8a6a1e 89 tmp[0] = pc.getc();
mfurukawa 0:caf1cb8a6a1e 90 dOut_led = !dOut_led;
mfurukawa 0:caf1cb8a6a1e 91 }
mfurukawa 0:caf1cb8a6a1e 92 }
mfurukawa 0:caf1cb8a6a1e 93 }